diff options
Diffstat (limited to 'src/modules')
131 files changed, 8068 insertions, 3599 deletions
diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index be465ab76..7a4e7a766 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -1,7 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. - * Author: Anton Babushkin <anton.babushkin@me.com> + * 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 diff --git a/src/modules/commander/accelerometer_calibration.h b/src/modules/commander/accelerometer_calibration.h index 1cf9c0977..6b7e16d44 100644 --- a/src/modules/commander/accelerometer_calibration.h +++ b/src/modules/commander/accelerometer_calibration.h @@ -1,7 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. - * Author: Anton Babushkin <anton.babushkin@me.com> + * 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 diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp index 5d21d89d0..0e58c68b6 100644 --- a/src/modules/commander/airspeed_calibration.cpp +++ b/src/modules/commander/airspeed_calibration.cpp @@ -51,6 +51,7 @@ #include <mavlink/mavlink_log.h> #include <systemlib/param/param.h> #include <systemlib/err.h> +#include <systemlib/airspeed.h> /* oddly, ERROR is not defined for c++ */ #ifdef ERROR @@ -64,19 +65,17 @@ int do_airspeed_calibration(int mavlink_fd) { /* give directions */ mavlink_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name); - mavlink_log_info(mavlink_fd, "ensure airspeed sensor is not registering wind"); - const int calibration_count = 2000; + const unsigned calibration_count = 2000; int diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure)); struct differential_pressure_s diff_pres; - int calibration_counter = 0; float diff_pres_offset = 0.0f; /* Reset sensor parameters */ struct airspeed_scale airscale = { - 0.0f, + diff_pres_offset, 1.0f, }; @@ -95,12 +94,29 @@ int do_airspeed_calibration(int mavlink_fd) } if (!paramreset_successful) { - warn("FAILED to set scale / offsets for airspeed"); - mavlink_log_critical(mavlink_fd, "dpress reset failed"); - mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name); - return ERROR; + + /* only warn if analog scaling is zero */ + float analog_scaling = 0.0f; + param_get(param_find("SENS_DPRES_ANSC"), &(analog_scaling)); + if (fabsf(analog_scaling) < 0.1f) { + mavlink_log_critical(mavlink_fd, "If analog sens, retry with [SENS_DPRES_ANSC=1000]"); + close(diff_pres_sub); + return ERROR; + } + + /* set scaling offset parameter */ + if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) { + mavlink_log_critical(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG); + close(diff_pres_sub); + return ERROR; + } } + unsigned calibration_counter = 0; + + mavlink_log_critical(mavlink_fd, "Ensure sensor is not measuring wind"); + usleep(500 * 1000); + while (calibration_counter < calibration_count) { /* wait blocking for new data */ @@ -112,11 +128,12 @@ int do_airspeed_calibration(int mavlink_fd) if (poll_ret) { orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres); + diff_pres_offset += diff_pres.differential_pressure_raw_pa; calibration_counter++; if (calibration_counter % (calibration_count / 20) == 0) { - mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, (calibration_counter * 100) / calibration_count); + mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, (calibration_counter * 80) / calibration_count); } } else if (poll_ret == 0) { @@ -131,6 +148,16 @@ int do_airspeed_calibration(int mavlink_fd) if (isfinite(diff_pres_offset)) { + int fd_scale = open(AIRSPEED_DEVICE_PATH, 0); + airscale.offset_pa = diff_pres_offset; + if (fd_scale > 0) { + if (OK != ioctl(fd_scale, AIRSPEEDIOCSSCALE, (long unsigned int)&airscale)) { + mavlink_log_critical(mavlink_fd, "airspeed offset update failed"); + } + + close(fd_scale); + } + if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) { mavlink_log_critical(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG); close(diff_pres_sub); @@ -147,14 +174,91 @@ int do_airspeed_calibration(int mavlink_fd) return ERROR; } - mavlink_log_info(mavlink_fd, CAL_DONE_MSG, sensor_name); - tune_neutral(true); - close(diff_pres_sub); - return OK; - } else { mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name); close(diff_pres_sub); return ERROR; } + + mavlink_log_critical(mavlink_fd, "Offset of %d Pa, create airflow now!", (int)diff_pres_offset); + + /* wait 500 ms to ensure parameter propagated through the system */ + usleep(500 * 1000); + + calibration_counter = 0; + const unsigned maxcount = 3000; + + /* just take a few samples and make sure pitot tubes are not reversed, timeout after ~30 seconds */ + while (calibration_counter < maxcount) { + + /* wait blocking for new data */ + struct pollfd fds[1]; + fds[0].fd = diff_pres_sub; + fds[0].events = POLLIN; + + int poll_ret = poll(fds, 1, 1000); + + if (poll_ret) { + orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres); + + calibration_counter++; + + if (fabsf(diff_pres.differential_pressure_raw_pa) < 50.0f) { + if (calibration_counter % 100 == 0) { + mavlink_log_critical(mavlink_fd, "Missing airflow! (%d, wanted: 50 Pa, #h101)", + (int)diff_pres.differential_pressure_raw_pa); + } + continue; + } + + /* do not allow negative values */ + if (diff_pres.differential_pressure_raw_pa < 0.0f) { + mavlink_log_info(mavlink_fd, "negative pressure: ERROR (%d Pa)", + (int)diff_pres.differential_pressure_raw_pa); + mavlink_log_critical(mavlink_fd, "%d Pa: swap static and dynamic ports!", (int)diff_pres.differential_pressure_raw_pa); + close(diff_pres_sub); + + /* the user setup is wrong, wipe the calibration to force a proper re-calibration */ + + diff_pres_offset = 0.0f; + if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) { + mavlink_log_critical(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG); + close(diff_pres_sub); + return ERROR; + } + + /* save */ + mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 0); + (void)param_save_default(); + + close(diff_pres_sub); + + mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name); + return ERROR; + } else { + mavlink_log_info(mavlink_fd, "positive pressure: OK (%d Pa)", + (int)diff_pres.differential_pressure_raw_pa); + break; + } + + } else if (poll_ret == 0) { + /* any poll failure for 1s is a reason to abort */ + mavlink_log_critical(mavlink_fd, CAL_FAILED_MSG, sensor_name); + close(diff_pres_sub); + return ERROR; + } + } + + if (calibration_counter == maxcount) { + mavlink_log_critical(mavlink_fd, CAL_FAILED_MSG, sensor_name); + close(diff_pres_sub); + return ERROR; + } + + mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 100); + + mavlink_log_info(mavlink_fd, CAL_DONE_MSG, sensor_name); + tune_neutral(true); + close(diff_pres_sub); + return OK; } diff --git a/src/modules/commander/airspeed_calibration.h b/src/modules/commander/airspeed_calibration.h index 71c701fc2..8c6b65ff1 100644 --- a/src/modules/commander/airspeed_calibration.h +++ b/src/modules/commander/airspeed_calibration.h @@ -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 diff --git a/src/modules/commander/baro_calibration.cpp b/src/modules/commander/baro_calibration.cpp index 3123c4087..da98644b4 100644 --- a/src/modules/commander/baro_calibration.cpp +++ b/src/modules/commander/baro_calibration.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * 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 diff --git a/src/modules/commander/baro_calibration.h b/src/modules/commander/baro_calibration.h index bc42bc6cf..ce78ae700 100644 --- a/src/modules/commander/baro_calibration.h +++ b/src/modules/commander/baro_calibration.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * 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 diff --git a/src/modules/commander/calibration_messages.h b/src/modules/commander/calibration_messages.h index fd8b8564d..b1e209efc 100644 --- a/src/modules/commander/calibration_messages.h +++ b/src/modules/commander/calibration_messages.h @@ -1,7 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. - * Author: Anton Babushkin <anton.babushkin@me.com> + * 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 diff --git a/src/modules/commander/calibration_routines.cpp b/src/modules/commander/calibration_routines.cpp index 43f341ae7..5796204bf 100644 --- a/src/modules/commander/calibration_routines.cpp +++ b/src/modules/commander/calibration_routines.cpp @@ -1,7 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: Lorenz Meier <lm@inf.ethz.ch> + * 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 diff --git a/src/modules/commander/calibration_routines.h b/src/modules/commander/calibration_routines.h index e3e7fbafd..3c8e49ee9 100644 --- a/src/modules/commander/calibration_routines.h +++ b/src/modules/commander/calibration_routines.h @@ -1,7 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: Lorenz Meier <lm@inf.ethz.ch> + * 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 diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index d7425cbb4..02571f56f 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -79,6 +79,7 @@ #include <uORB/topics/differential_pressure.h> #include <uORB/topics/safety.h> #include <uORB/topics/system_power.h> +#include <uORB/topics/mission.h> #include <uORB/topics/mission_result.h> #include <uORB/topics/telemetry_status.h> @@ -94,6 +95,7 @@ #include <systemlib/rc_check.h> #include <geo/geo.h> #include <systemlib/state_table.h> +#include <dataman/dataman.h> #include "px4_custom_mode.h" #include "commander_helper.h" @@ -124,10 +126,11 @@ extern struct system_load_s system_load; #define STICK_ON_OFF_HYSTERESIS_TIME_MS 1000 #define STICK_ON_OFF_COUNTER_LIMIT (STICK_ON_OFF_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC) -#define POSITION_TIMEOUT (600 * 1000) /**< consider the local or global position estimate invalid after 600ms */ +#define POSITION_TIMEOUT (2 * 1000 * 1000) /**< consider the local or global position estimate invalid after 600ms */ #define FAILSAFE_DEFAULT_TIMEOUT (3 * 1000 * 1000) /**< hysteresis time - the failsafe will trigger after 3 seconds in this state */ #define RC_TIMEOUT 500000 #define DL_TIMEOUT 5 * 1000* 1000 +#define OFFBOARD_TIMEOUT 500000 #define DIFFPRESS_TIMEOUT 2000000 #define PRINT_INTERVAL 5000000 @@ -162,12 +165,14 @@ static bool on_usb_power = false; static float takeoff_alt = 5.0f; static int parachute_enabled = 0; -static float eph_epv_threshold = 5.0f; +static float eph_threshold = 5.0f; +static float epv_threshold = 10.0f; static struct vehicle_status_s status; static struct actuator_armed_s armed; static struct safety_s safety; static struct vehicle_control_mode_s control_mode; +static struct offboard_control_setpoint_s sp_offboard; /* tasks waiting for low prio thread */ typedef enum { @@ -286,15 +291,22 @@ int commander_main(int argc, char *argv[]) exit(0); } - if (!strcmp(argv[1], "status")) { - if (thread_running) { - warnx("\tcommander is running"); - print_status(); + /* commands needing the app to run below */ + if (!thread_running) { + warnx("\tcommander not started"); + exit(1); + } - } else { - warnx("\tcommander not started"); - } + if (!strcmp(argv[1], "status")) { + print_status(); + exit(0); + } + if (!strcmp(argv[1], "check")) { + int mavlink_fd_local = open(MAVLINK_LOG_DEVICE, 0); + int checkres = prearm_check(&status, mavlink_fd_local); + close(mavlink_fd_local); + warnx("FINAL RESULT: %s", (checkres == 0) ? "OK" : "FAILED"); exit(0); } @@ -303,7 +315,7 @@ int commander_main(int argc, char *argv[]) exit(0); } - if (!strcmp(argv[1], "2")) { + if (!strcmp(argv[1], "disarm")) { arm_disarm(false, mavlink_fd, "command line"); exit(0); } @@ -324,7 +336,9 @@ void usage(const char *reason) void print_status() { + warnx("type: %s", (status.is_rotary_wing) ? "ROTARY" : "PLANE"); warnx("usb powered: %s", (on_usb_power) ? "yes" : "no"); + warnx("avionics rail: %6.2f V", (double)status.avionics_power_rail_voltage); /* read all relevant states */ int state_sub = orb_subscribe(ORB_ID(vehicle_status)); @@ -381,7 +395,7 @@ transition_result_t arm_disarm(bool arm, const int mavlink_fd_local, const char // Transition the armed state. By passing mavlink_fd to arming_state_transition it will // output appropriate error messages if the state cannot transition. - arming_res = arming_state_transition(&status, &safety, arm ? ARMING_STATE_ARMED : ARMING_STATE_STANDBY, &armed, mavlink_fd_local); + arming_res = arming_state_transition(&status, &safety, arm ? ARMING_STATE_ARMED : ARMING_STATE_STANDBY, &armed, true /* fRunPreArmChecks */, mavlink_fd_local); if (arming_res == TRANSITION_CHANGED && mavlink_fd) { mavlink_log_info(mavlink_fd_local, "[cmd] %s by %s", arm ? "ARMED" : "DISARMED", armedBy); @@ -393,10 +407,12 @@ transition_result_t arm_disarm(bool arm, const int mavlink_fd_local, const char return arming_res; } -bool handle_command(struct vehicle_status_s *status, const struct safety_s *safety, struct vehicle_command_s *cmd, struct actuator_armed_s *armed, struct home_position_s *home, struct vehicle_global_position_s *global_pos, orb_advert_t *home_pub) +bool handle_command(struct vehicle_status_s *status_local, const struct safety_s *safety_local, + struct vehicle_command_s *cmd, struct actuator_armed_s *armed_local, + struct home_position_s *home, struct vehicle_global_position_s *global_pos, orb_advert_t *home_pub) { /* only handle commands that are meant to be handled by this system and component */ - if (cmd->target_system != status->system_id || ((cmd->target_component != status->component_id) && (cmd->target_component != 0))) { // component_id 0: valid for all components + if (cmd->target_system != status_local->system_id || ((cmd->target_component != status_local->component_id) && (cmd->target_component != 0))) { // component_id 0: valid for all components return false; } @@ -415,7 +431,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe /* set HIL state */ hil_state_t new_hil_state = (base_mode & MAV_MODE_FLAG_HIL_ENABLED) ? HIL_STATE_ON : HIL_STATE_OFF; - transition_result_t hil_ret = hil_state_transition(new_hil_state, status_pub, status, mavlink_fd); + transition_result_t hil_ret = hil_state_transition(new_hil_state, status_pub, status_local, mavlink_fd); // Transition the arming state arming_ret = arm_disarm(base_mode & MAV_MODE_FLAG_SAFETY_ARMED, mavlink_fd, "set mode command"); @@ -424,39 +440,43 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe /* use autopilot-specific mode */ if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_MANUAL) { /* MANUAL */ - main_ret = main_state_transition(status, MAIN_STATE_MANUAL); + main_ret = main_state_transition(status_local, MAIN_STATE_MANUAL); } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_ALTCTL) { /* ALTCTL */ - main_ret = main_state_transition(status, MAIN_STATE_ALTCTL); + main_ret = main_state_transition(status_local, MAIN_STATE_ALTCTL); } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_POSCTL) { /* POSCTL */ - main_ret = main_state_transition(status, MAIN_STATE_POSCTL); + main_ret = main_state_transition(status_local, MAIN_STATE_POSCTL); } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_AUTO) { /* AUTO */ - main_ret = main_state_transition(status, MAIN_STATE_AUTO_MISSION); + main_ret = main_state_transition(status_local, MAIN_STATE_AUTO_MISSION); } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_ACRO) { /* ACRO */ - main_ret = main_state_transition(status, MAIN_STATE_ACRO); + main_ret = main_state_transition(status_local, MAIN_STATE_ACRO); + + } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_OFFBOARD) { + /* OFFBOARD */ + main_ret = main_state_transition(status_local, MAIN_STATE_OFFBOARD); } } else { /* use base mode */ if (base_mode & MAV_MODE_FLAG_AUTO_ENABLED) { /* AUTO */ - main_ret = main_state_transition(status, MAIN_STATE_AUTO_MISSION); + main_ret = main_state_transition(status_local, MAIN_STATE_AUTO_MISSION); } else if (base_mode & MAV_MODE_FLAG_MANUAL_INPUT_ENABLED) { if (base_mode & MAV_MODE_FLAG_GUIDED_ENABLED) { /* POSCTL */ - main_ret = main_state_transition(status, MAIN_STATE_POSCTL); + main_ret = main_state_transition(status_local, MAIN_STATE_POSCTL); } else if (base_mode & MAV_MODE_FLAG_STABILIZE_ENABLED) { /* MANUAL */ - main_ret = main_state_transition(status, MAIN_STATE_MANUAL); + main_ret = main_state_transition(status_local, MAIN_STATE_MANUAL); } } } @@ -471,22 +491,25 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe break; case VEHICLE_CMD_COMPONENT_ARM_DISARM: { - // 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); + // Adhere to MAVLink specs, but base on knowledge that these fundamentally encode ints + // for logic state parameters + + if (static_cast<int>(cmd->param1 + 0.5f) != 0 && static_cast<int>(cmd->param1 + 0.5f) != 1) { + mavlink_log_critical(mavlink_fd, "Unsupported ARM_DISARM param: %.3f", (double)cmd->param1); } else { + bool cmd_arms = (static_cast<int>(cmd->param1 + 0.5f) == 1); + // Flick to inair restore first if this comes from an onboard system - if (cmd->source_system == status->system_id && cmd->source_component == status->component_id) { - status->arming_state = ARMING_STATE_IN_AIR_RESTORE; + if (cmd->source_system == status_local->system_id && cmd->source_component == status_local->component_id) { + status_local->arming_state = ARMING_STATE_IN_AIR_RESTORE; } - transition_result_t arming_res = arm_disarm(cmd->param1 != 0.0f, mavlink_fd, "arm/disarm component command"); + transition_result_t arming_res = arm_disarm(cmd_arms, mavlink_fd, "arm/disarm component command"); if (arming_res == TRANSITION_DENIED) { - mavlink_log_critical(mavlink_fd, "#audio: REJECTING component arm cmd"); + mavlink_log_critical(mavlink_fd, "REJECTING component arm cmd"); cmd_result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; } else { @@ -498,20 +521,24 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe case VEHICLE_CMD_OVERRIDE_GOTO: { // TODO listen vehicle_command topic directly from navigator (?) - unsigned int mav_goto = cmd->param1; + + // Increase by 0.5f and rely on the integer cast + // implicit floor(). This is the *safest* way to + // convert from floats representing small ints to actual ints. + unsigned int mav_goto = (cmd->param1 + 0.5f); if (mav_goto == 0) { // MAV_GOTO_DO_HOLD - status->nav_state = NAVIGATION_STATE_AUTO_LOITER; - mavlink_log_critical(mavlink_fd, "#audio: pause mission cmd"); + status_local->nav_state = NAVIGATION_STATE_AUTO_LOITER; + mavlink_log_critical(mavlink_fd, "Pause mission cmd"); cmd_result = VEHICLE_CMD_RESULT_ACCEPTED; } else if (mav_goto == 1) { // MAV_GOTO_DO_CONTINUE - status->nav_state = NAVIGATION_STATE_AUTO_MISSION; - mavlink_log_critical(mavlink_fd, "#audio: continue mission cmd"); + status_local->nav_state = NAVIGATION_STATE_AUTO_MISSION; + mavlink_log_critical(mavlink_fd, "Continue mission cmd"); cmd_result = VEHICLE_CMD_RESULT_ACCEPTED; } else { - mavlink_log_info(mavlink_fd, "Unsupported OVERRIDE_GOTO: %f %f %f %f %f %f %f %f", + mavlink_log_critical(mavlink_fd, "REJ CMD: %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f", (double)cmd->param1, (double)cmd->param2, (double)cmd->param3, @@ -523,31 +550,26 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe } break; -#if 0 /* Flight termination */ - case VEHICLE_CMD_DO_SET_SERVO: { //xxx: needs its own mavlink command - - //XXX: to enable the parachute, a param needs to be set - //xxx: for safety only for now, param3 is unused by VEHICLE_CMD_DO_SET_SERVO - if (armed->armed && cmd->param3 > 0.5 && parachute_enabled) { - transition_result_t failsafe_res = failsafe_state_transition(status, FAILSAFE_STATE_TERMINATION); - cmd_result = VEHICLE_CMD_RESULT_ACCEPTED; - + case VEHICLE_CMD_DO_FLIGHTTERMINATION: { + if (cmd->param1 > 0.5f) { + //XXX update state machine? + armed_local->force_failsafe = true; + warnx("forcing failsafe"); } else { - /* reject parachute depoyment not armed */ - cmd_result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + armed_local->force_failsafe = false; + warnx("disabling failsafe"); } - + cmd_result = VEHICLE_CMD_RESULT_ACCEPTED; } break; -#endif case VEHICLE_CMD_DO_SET_HOME: { bool use_current = cmd->param1 > 0.5f; if (use_current) { /* use current position */ - if (status->condition_global_position_valid) { + if (status_local->condition_global_position_valid) { home->lat = global_pos->lat; home->lon = global_pos->lon; home->alt = global_pos->alt; @@ -584,7 +606,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe } /* mark home position as set */ - status->condition_home_position_valid = true; + status_local->condition_home_position_valid = true; } } break; @@ -636,7 +658,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"; @@ -644,8 +666,9 @@ int commander_thread_main(int argc, char *argv[]) main_states_str[MAIN_STATE_AUTO_LOITER] = "AUTO_LOITER"; main_states_str[MAIN_STATE_AUTO_RTL] = "AUTO_RTL"; main_states_str[MAIN_STATE_ACRO] = "ACRO"; + main_states_str[MAIN_STATE_OFFBOARD] = "OFFBOARD"; - 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"; @@ -654,7 +677,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"; @@ -666,17 +689,18 @@ int commander_thread_main(int argc, char *argv[]) nav_states_str[NAVIGATION_STATE_LAND] = "LAND"; nav_states_str[NAVIGATION_STATE_DESCEND] = "DESCEND"; nav_states_str[NAVIGATION_STATE_TERMINATION] = "TERMINATION"; + nav_states_str[NAVIGATION_STATE_OFFBOARD] = "OFFBOARD"; /* pthread for slow low prio thread */ pthread_t commander_low_prio_thread; /* initialize */ if (led_init() != 0) { - warnx("ERROR: Failed to initialize leds"); + warnx("ERROR: LED INIT FAIL"); } if (buzzer_init() != OK) { - warnx("ERROR: Failed to initialize buzzer"); + warnx("ERROR: BUZZER INIT FAIL"); } mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); @@ -716,9 +740,15 @@ int commander_thread_main(int argc, char *argv[]) // CIRCUIT BREAKERS status.circuit_breaker_engaged_power_check = false; + status.circuit_breaker_engaged_airspd_check = false; /* publish initial state */ status_pub = orb_advertise(ORB_ID(vehicle_status), &status); + if (status_pub < 0) { + warnx("ERROR: orb_advertise for topic vehicle_status failed (uorb app running?).\n"); + warnx("exiting."); + exit(ERROR); + } /* armed topic */ orb_advert_t armed_pub; @@ -736,13 +766,29 @@ int commander_thread_main(int argc, char *argv[]) struct home_position_s home; memset(&home, 0, sizeof(home)); - if (status_pub < 0) { - warnx("ERROR: orb_advertise for topic vehicle_status failed (uorb app running?).\n"); - warnx("exiting."); - exit(ERROR); - } + /* init mission state, do it here to allow navigator to use stored mission even if mavlink failed to start */ + orb_advert_t mission_pub = -1; + mission_s mission; + if (dm_read(DM_KEY_MISSION_STATE, 0, &mission, sizeof(mission_s)) == sizeof(mission_s)) { + if (mission.dataman_id >= 0 && mission.dataman_id <= 1) { + warnx("loaded mission state: dataman_id=%d, count=%u, current=%d", mission.dataman_id, mission.count, mission.current_seq); + mavlink_log_info(mavlink_fd, "[cmd] dataman_id=%d, count=%u, current=%d", + mission.dataman_id, mission.count, mission.current_seq); + } else { + const char *missionfail = "reading mission state failed"; + warnx("%s", missionfail); + mavlink_log_critical(mavlink_fd, missionfail); + + /* initialize mission state in dataman */ + mission.dataman_id = 0; + mission.count = 0; + mission.current_seq = 0; + dm_write(DM_KEY_MISSION_STATE, 0, DM_PERSIST_POWER_ON_RESET, &mission, sizeof(mission_s)); + } - mavlink_log_info(mavlink_fd, "[cmd] started"); + mission_pub = orb_advertise(ORB_ID(offboard_mission), &mission); + orb_publish(ORB_ID(offboard_mission), mission_pub, &mission); + } int ret; @@ -775,7 +821,7 @@ int commander_thread_main(int argc, char *argv[]) bool updated = false; - bool rc_calibration_ok = (OK == rc_calibration_check(mavlink_fd)); + rc_calibration_check(mavlink_fd); /* Subscribe to safety topic */ int safety_sub = orb_subscribe(ORB_ID(safety)); @@ -795,13 +841,18 @@ int commander_thread_main(int argc, char *argv[]) /* Subscribe to offboard control data */ int sp_offboard_sub = orb_subscribe(ORB_ID(offboard_control_setpoint)); - struct offboard_control_setpoint_s sp_offboard; memset(&sp_offboard, 0, sizeof(sp_offboard)); - /* Subscribe to telemetry status */ - int telemetry_sub = orb_subscribe(ORB_ID(telemetry_status)); - struct telemetry_status_s telemetry; - memset(&telemetry, 0, sizeof(telemetry)); + /* Subscribe to telemetry status topics */ + int telemetry_subs[TELEMETRY_STATUS_ORB_ID_NUM]; + uint64_t telemetry_last_heartbeat[TELEMETRY_STATUS_ORB_ID_NUM]; + bool telemetry_lost[TELEMETRY_STATUS_ORB_ID_NUM]; + + for (int i = 0; i < TELEMETRY_STATUS_ORB_ID_NUM; i++) { + telemetry_subs[i] = orb_subscribe(telemetry_status_orb_id[i]); + telemetry_last_heartbeat[i] = 0; + telemetry_lost[i] = true; + } /* Subscribe to global position */ int global_position_sub = orb_subscribe(ORB_ID(vehicle_global_position)); @@ -869,6 +920,11 @@ int commander_thread_main(int argc, char *argv[]) struct system_power_s system_power; memset(&system_power, 0, sizeof(system_power)); + /* Subscribe to actuator controls (outputs) */ + int actuator_controls_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS); + struct actuator_controls_s actuator_controls; + memset(&actuator_controls, 0, sizeof(actuator_controls)); + control_status_leds(&status, &armed, true); /* now initialized */ @@ -885,7 +941,6 @@ int commander_thread_main(int argc, char *argv[]) bool arming_state_changed = false; bool main_state_changed = false; bool failsafe_old = false; - bool system_checked = false; while (!thread_should_exit) { @@ -929,11 +984,12 @@ int commander_thread_main(int argc, char *argv[]) param_get(_param_component_id, &(status.component_id)); status.circuit_breaker_engaged_power_check = circuit_breaker_enabled("CBRK_SUPPLY_CHK", CBRK_SUPPLY_CHK_KEY); + status.circuit_breaker_engaged_airspd_check = circuit_breaker_enabled("CBRK_AIRSPD_CHK", CBRK_AIRSPD_CHK_KEY); status_changed = true; /* re-check RC calibration */ - rc_calibration_ok = (OK == rc_calibration_check(mavlink_fd)); + rc_calibration_check(mavlink_fd); } /* navigation parameters */ @@ -942,15 +998,6 @@ int commander_thread_main(int argc, char *argv[]) param_get(_param_enable_datalink_loss, &datalink_loss_enabled); } - /* Perform system checks (again) once params are loaded and MAVLink is up. */ - if (!system_checked && mavlink_fd && - (telemetry.heartbeat_time > 0) && - (hrt_elapsed_time(&telemetry.heartbeat_time) < 1 * 1000 * 1000)) { - - (void)rc_calibration_check(mavlink_fd); - system_checked = true; - } - orb_check(sp_man_sub, &updated); if (updated) { @@ -963,10 +1010,39 @@ int commander_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(offboard_control_setpoint), sp_offboard_sub, &sp_offboard); } - orb_check(telemetry_sub, &updated); + if (sp_offboard.timestamp != 0 && + sp_offboard.timestamp + OFFBOARD_TIMEOUT > hrt_absolute_time()) { + if (status.offboard_control_signal_lost) { + status.offboard_control_signal_lost = false; + status_changed = true; + } + } else { + if (!status.offboard_control_signal_lost) { + status.offboard_control_signal_lost = true; + status_changed = true; + } + } + + for (int i = 0; i < TELEMETRY_STATUS_ORB_ID_NUM; i++) { + orb_check(telemetry_subs[i], &updated); - if (updated) { - orb_copy(ORB_ID(telemetry_status), telemetry_sub, &telemetry); + if (updated) { + struct telemetry_status_s telemetry; + memset(&telemetry, 0, sizeof(telemetry)); + + orb_copy(telemetry_status_orb_id[i], telemetry_subs[i], &telemetry); + + /* perform system checks when new telemetry link connected */ + if (mavlink_fd && + telemetry_last_heartbeat[i] == 0 && + telemetry.heartbeat_time > 0 && + hrt_elapsed_time(&telemetry.heartbeat_time) < DL_TIMEOUT) { + + (void)rc_calibration_check(mavlink_fd); + } + + telemetry_last_heartbeat[i] = telemetry.heartbeat_time; + } } orb_check(sensor_sub, &updated); @@ -1013,8 +1089,8 @@ int commander_thread_main(int argc, char *argv[]) if (status.hil_state == HIL_STATE_OFF && safety.safety_switch_available && !safety.safety_off && armed.armed) { arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR); - if (TRANSITION_CHANGED == arming_state_transition(&status, &safety, new_arming_state, &armed, mavlink_fd)) { - mavlink_log_info(mavlink_fd, "[cmd] DISARMED by safety switch"); + if (TRANSITION_CHANGED == arming_state_transition(&status, &safety, new_arming_state, &armed, true /* fRunPreArmChecks */, mavlink_fd)) { + mavlink_log_info(mavlink_fd, "DISARMED by safety switch"); arming_state_changed = true; } } @@ -1038,32 +1114,32 @@ int commander_thread_main(int argc, char *argv[]) /* update condition_global_position_valid */ /* hysteresis for EPH/EPV */ - bool eph_epv_good; + bool eph_good; if (status.condition_global_position_valid) { - if (global_position.eph > eph_epv_threshold * 2.0f || global_position.epv > eph_epv_threshold * 2.0f) { - eph_epv_good = false; + if (global_position.eph > eph_threshold * 2.5f) { + eph_good = false; } else { - eph_epv_good = true; + eph_good = true; } } else { - if (global_position.eph < eph_epv_threshold && global_position.epv < eph_epv_threshold) { - eph_epv_good = true; + if (global_position.eph < eph_threshold) { + eph_good = true; } else { - eph_epv_good = false; + eph_good = false; } } - check_valid(global_position.timestamp, POSITION_TIMEOUT, eph_epv_good, &(status.condition_global_position_valid), &status_changed); + check_valid(global_position.timestamp, POSITION_TIMEOUT, eph_good, &(status.condition_global_position_valid), &status_changed); /* check if GPS fix is ok */ /* update home position */ if (!status.condition_home_position_valid && status.condition_global_position_valid && !armed.armed && - (global_position.eph < eph_epv_threshold) && (global_position.epv < eph_epv_threshold)) { + (global_position.eph < eph_threshold) && (global_position.epv < epv_threshold)) { home.lat = global_position.lat; home.lon = global_position.lon; @@ -1093,8 +1169,8 @@ int commander_thread_main(int argc, char *argv[]) /* hysteresis for EPH */ bool local_eph_good; - if (status.condition_global_position_valid) { - if (local_position.eph > eph_epv_threshold * 2.0f) { + if (status.condition_local_position_valid) { + if (local_position.eph > eph_threshold * 2.5f) { local_eph_good = false; } else { @@ -1102,7 +1178,7 @@ int commander_thread_main(int argc, char *argv[]) } } else { - if (local_position.eph < eph_epv_threshold) { + if (local_position.eph < eph_threshold) { local_eph_good = true; } else { @@ -1118,10 +1194,10 @@ int commander_thread_main(int argc, char *argv[]) status_changed = true; if (status.condition_landed) { - mavlink_log_critical(mavlink_fd, "#audio: LANDED"); + mavlink_log_critical(mavlink_fd, "LANDED MODE"); } else { - mavlink_log_critical(mavlink_fd, "#audio: IN AIR"); + mavlink_log_critical(mavlink_fd, "IN AIR MODE"); } } } @@ -1131,13 +1207,17 @@ int commander_thread_main(int argc, char *argv[]) if (updated) { orb_copy(ORB_ID(battery_status), battery_sub, &battery); + orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_controls_sub, &actuator_controls); /* only consider battery voltage if system has been running 2s and battery voltage is valid */ if (hrt_absolute_time() > start_time + 2000000 && battery.voltage_filtered_v > 0.0f) { status.battery_voltage = battery.voltage_filtered_v; status.battery_current = battery.current_a; status.condition_battery_voltage_valid = true; - status.battery_remaining = battery_remaining_estimate_voltage(battery.voltage_filtered_v, battery.discharged_mah); + + /* get throttle (if armed), as we only care about energy negative throttle also counts */ + float throttle = (armed.armed) ? fabsf(actuator_controls.control[3]) : 0.0f; + status.battery_remaining = battery_remaining_estimate_voltage(battery.voltage_filtered_v, battery.discharged_mah, throttle); } } @@ -1199,27 +1279,27 @@ int commander_thread_main(int argc, char *argv[]) } /* if battery voltage is getting lower, warn using buzzer, etc. */ - if (status.condition_battery_voltage_valid && status.battery_remaining < 0.25f && !low_battery_voltage_actions_done) { + if (status.condition_battery_voltage_valid && status.battery_remaining < 0.18f && !low_battery_voltage_actions_done) { low_battery_voltage_actions_done = true; - mavlink_log_critical(mavlink_fd, "#audio: WARNING: LOW BATTERY"); + mavlink_log_critical(mavlink_fd, "LOW BATTERY, RETURN TO LAND ADVISED"); status.battery_warning = VEHICLE_BATTERY_WARNING_LOW; status_changed = true; - } else if (status.condition_battery_voltage_valid && status.battery_remaining < 0.1f && !critical_battery_voltage_actions_done && low_battery_voltage_actions_done) { + } else if (status.condition_battery_voltage_valid && status.battery_remaining < 0.09f && !critical_battery_voltage_actions_done && low_battery_voltage_actions_done) { /* critical battery voltage, this is rather an emergency, change state machine */ critical_battery_voltage_actions_done = true; - mavlink_log_critical(mavlink_fd, "#audio: EMERGENCY: CRITICAL BATTERY"); + mavlink_log_emergency(mavlink_fd, "CRITICAL BATTERY, LAND IMMEDIATELY"); status.battery_warning = VEHICLE_BATTERY_WARNING_CRITICAL; if (armed.armed) { - arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_ARMED_ERROR, &armed, mavlink_fd); + arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_ARMED_ERROR, &armed, true /* fRunPreArmChecks */, mavlink_fd); if (arming_ret == TRANSITION_CHANGED) { arming_state_changed = true; } } else { - arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_STANDBY_ERROR, &armed, mavlink_fd); + arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_STANDBY_ERROR, &armed, true /* fRunPreArmChecks */, mavlink_fd); if (arming_ret == TRANSITION_CHANGED) { arming_state_changed = true; @@ -1233,7 +1313,7 @@ int commander_thread_main(int argc, char *argv[]) /* If in INIT state, try to proceed to STANDBY state */ if (status.arming_state == ARMING_STATE_INIT && low_prio_task == LOW_PRIO_TASK_NONE) { /* TODO: check for sensors */ - arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed, mavlink_fd); + arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed, true /* fRunPreArmChecks */, mavlink_fd); if (arming_ret == TRANSITION_CHANGED) { arming_state_changed = true; @@ -1280,12 +1360,12 @@ int commander_thread_main(int argc, char *argv[]) /* handle the case where RC signal was regained */ if (!status.rc_signal_found_once) { status.rc_signal_found_once = true; - mavlink_log_critical(mavlink_fd, "#audio: detected RC signal first time"); + mavlink_log_critical(mavlink_fd, "detected RC signal first time"); status_changed = true; } else { if (status.rc_signal_lost) { - mavlink_log_critical(mavlink_fd, "#audio: RC signal regained"); + mavlink_log_critical(mavlink_fd, "RC signal regained"); status_changed = true; } } @@ -1302,7 +1382,7 @@ int commander_thread_main(int argc, char *argv[]) if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) { /* disarm to STANDBY if ARMED or to STANDBY_ERROR if ARMED_ERROR */ arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR); - arming_ret = arming_state_transition(&status, &safety, new_arming_state, &armed, mavlink_fd); + arming_ret = arming_state_transition(&status, &safety, new_arming_state, &armed, true /* fRunPreArmChecks */, mavlink_fd); if (arming_ret == TRANSITION_CHANGED) { arming_state_changed = true; } @@ -1326,9 +1406,9 @@ int commander_thread_main(int argc, char *argv[]) * the system can be armed in auto if armed via the GCS. */ if (status.main_state != MAIN_STATE_MANUAL) { - print_reject_arm("#audio: NOT ARMING: Switch to MANUAL mode first."); + print_reject_arm("NOT ARMING: Switch to MANUAL mode first."); } else { - arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed, mavlink_fd); + arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed, true /* fRunPreArmChecks */, mavlink_fd); if (arming_ret == TRANSITION_CHANGED) { arming_state_changed = true; } @@ -1346,16 +1426,20 @@ int commander_thread_main(int argc, char *argv[]) if (arming_ret == TRANSITION_CHANGED) { if (status.arming_state == ARMING_STATE_ARMED) { - mavlink_log_info(mavlink_fd, "[cmd] ARMED by RC"); + mavlink_log_info(mavlink_fd, "ARMED by RC"); } else { - mavlink_log_info(mavlink_fd, "[cmd] DISARMED by RC"); + mavlink_log_info(mavlink_fd, "DISARMED by RC"); } arming_state_changed = true; } else if (arming_ret == TRANSITION_DENIED) { - /* DENIED here indicates bug in the commander */ - mavlink_log_critical(mavlink_fd, "ERROR: arming state transition denied"); + /* + * the arming transition can be denied to a number of reasons: + * - pre-flight check failed (sensors not ok or not calibrated) + * - safety not disabled + * - system not in manual mode + */ tune_negative(true); } @@ -1369,29 +1453,46 @@ int commander_thread_main(int argc, char *argv[]) } else if (main_res == TRANSITION_DENIED) { /* DENIED here indicates bug in the commander */ - mavlink_log_critical(mavlink_fd, "ERROR: main state transition denied"); + mavlink_log_critical(mavlink_fd, "main state transition denied"); } } else { if (!status.rc_signal_lost) { - mavlink_log_critical(mavlink_fd, "#audio: CRITICAL: RC SIGNAL LOST"); + mavlink_log_critical(mavlink_fd, "RC SIGNAL LOST"); status.rc_signal_lost = true; status_changed = true; } } - /* data link check */ - if (hrt_absolute_time() < telemetry.heartbeat_time + DL_TIMEOUT) { + /* data links check */ + bool have_link = false; + for (int i = 0; i < TELEMETRY_STATUS_ORB_ID_NUM; i++) { + if (telemetry_last_heartbeat[i] != 0 && hrt_elapsed_time(&telemetry_last_heartbeat[i]) < DL_TIMEOUT) { + /* handle the case where data link was regained */ + if (telemetry_lost[i]) { + mavlink_log_critical(mavlink_fd, "data link %i regained", i); + telemetry_lost[i] = false; + } + have_link = true; + + } else { + if (!telemetry_lost[i]) { + mavlink_log_critical(mavlink_fd, "data link %i lost", i); + telemetry_lost[i] = true; + } + } + } + + if (have_link) { /* handle the case where data link was regained */ if (status.data_link_lost) { - mavlink_log_critical(mavlink_fd, "#audio: data link regained"); status.data_link_lost = false; status_changed = true; } } else { if (!status.data_link_lost) { - mavlink_log_critical(mavlink_fd, "#audio: CRITICAL: DATA LINK LOST"); + mavlink_log_critical(mavlink_fd, "ALL DATA LINKS LOST"); status.data_link_lost = true; status_changed = true; } @@ -1419,7 +1520,7 @@ int commander_thread_main(int argc, char *argv[]) /* update home position on arming if at least 2s from commander start spent to avoid setting home on in-air restart */ if (armed.armed && !was_armed && hrt_absolute_time() > start_time + 2000000 && status.condition_global_position_valid && - (global_position.eph < eph_epv_threshold) && (global_position.epv < eph_epv_threshold)) { + (global_position.eph < eph_threshold) && (global_position.epv < epv_threshold)) { // TODO remove code duplication home.lat = global_position.lat; @@ -1451,7 +1552,7 @@ int commander_thread_main(int argc, char *argv[]) /* now set navigation state according to failsafe and main state */ bool nav_state_changed = set_nav_state(&status, (bool)datalink_loss_enabled, - mission_result.mission_finished); + mission_result.finished); // TODO handle mode changes by commands if (main_state_changed) { @@ -1555,6 +1656,7 @@ int commander_thread_main(int argc, char *argv[]) close(diff_pres_sub); close(param_changed_sub); close(battery_sub); + close(mission_pub); thread_running = false; @@ -1574,22 +1676,22 @@ check_valid(hrt_abstime timestamp, hrt_abstime timeout, bool valid_in, bool *val } void -control_status_leds(vehicle_status_s *status, const actuator_armed_s *actuator_armed, bool changed) +control_status_leds(vehicle_status_s *status_local, const actuator_armed_s *actuator_armed, bool changed) { /* driving rgbled */ if (changed) { bool set_normal_color = false; /* set mode */ - if (status->arming_state == ARMING_STATE_ARMED) { + if (status_local->arming_state == ARMING_STATE_ARMED) { rgbled_set_mode(RGBLED_MODE_ON); set_normal_color = true; - } else if (status->arming_state == ARMING_STATE_ARMED_ERROR) { + } else if (status_local->arming_state == ARMING_STATE_ARMED_ERROR) { rgbled_set_mode(RGBLED_MODE_BLINK_FAST); rgbled_set_color(RGBLED_COLOR_RED); - } else if (status->arming_state == ARMING_STATE_STANDBY) { + } else if (status_local->arming_state == ARMING_STATE_STANDBY) { rgbled_set_mode(RGBLED_MODE_BREATHE); set_normal_color = true; @@ -1600,12 +1702,12 @@ control_status_leds(vehicle_status_s *status, const actuator_armed_s *actuator_a if (set_normal_color) { /* set color */ - if (status->battery_warning == VEHICLE_BATTERY_WARNING_LOW || status->failsafe) { + if (status_local->battery_warning == VEHICLE_BATTERY_WARNING_LOW || status_local->failsafe) { rgbled_set_color(RGBLED_COLOR_AMBER); /* VEHICLE_BATTERY_WARNING_CRITICAL handled as ARMING_STATE_ARMED_ERROR / ARMING_STATE_STANDBY_ERROR */ } else { - if (status->condition_local_position_valid) { + if (status_local->condition_local_position_valid) { rgbled_set_color(RGBLED_COLOR_GREEN); } else { @@ -1638,7 +1740,7 @@ control_status_leds(vehicle_status_s *status, const actuator_armed_s *actuator_a #endif /* give system warnings on error LED, XXX maybe add memory usage warning too */ - if (status->load > 0.95f) { + if (status_local->load > 0.95f) { if (leds_counter % 2 == 0) { led_toggle(LED_AMBER); } @@ -1651,11 +1753,23 @@ control_status_leds(vehicle_status_s *status, const actuator_armed_s *actuator_a } transition_result_t -set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoint_s *sp_man) +set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_setpoint_s *sp_man) { /* set main state according to RC switches */ transition_result_t res = TRANSITION_DENIED; + /* offboard switch overrides main switch */ + if (sp_man->offboard_switch == SWITCH_POS_ON) { + res = main_state_transition(status_local, MAIN_STATE_OFFBOARD); + if (res == TRANSITION_DENIED) { + print_reject_mode(status_local, "OFFBOARD"); + + } else { + return res; + } + } + + /* offboard switched off or denied, check main mode switch */ switch (sp_man->mode_switch) { case SWITCH_POS_NONE: res = TRANSITION_NOT_CHANGED; @@ -1663,93 +1777,100 @@ set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoin case SWITCH_POS_OFF: // MANUAL if (sp_man->acro_switch == SWITCH_POS_ON) { - res = main_state_transition(status, MAIN_STATE_ACRO); + res = main_state_transition(status_local, MAIN_STATE_ACRO); } else { - res = main_state_transition(status, MAIN_STATE_MANUAL); + res = main_state_transition(status_local, MAIN_STATE_MANUAL); } // TRANSITION_DENIED is not possible here break; case SWITCH_POS_MIDDLE: // ASSIST if (sp_man->posctl_switch == SWITCH_POS_ON) { - res = main_state_transition(status, MAIN_STATE_POSCTL); + res = main_state_transition(status_local, MAIN_STATE_POSCTL); if (res != TRANSITION_DENIED) { break; // changed successfully or already in this state } - print_reject_mode(status, "POSCTL"); + print_reject_mode(status_local, "POSCTL"); } // fallback to ALTCTL - res = main_state_transition(status, MAIN_STATE_ALTCTL); + res = main_state_transition(status_local, MAIN_STATE_ALTCTL); if (res != TRANSITION_DENIED) { break; // changed successfully or already in this mode } if (sp_man->posctl_switch != SWITCH_POS_ON) { - print_reject_mode(status, "ALTCTL"); + print_reject_mode(status_local, "ALTCTL"); } // fallback to MANUAL - res = main_state_transition(status, MAIN_STATE_MANUAL); + res = main_state_transition(status_local, MAIN_STATE_MANUAL); // TRANSITION_DENIED is not possible here break; case SWITCH_POS_ON: // AUTO if (sp_man->return_switch == SWITCH_POS_ON) { - res = main_state_transition(status, MAIN_STATE_AUTO_RTL); + res = main_state_transition(status_local, MAIN_STATE_AUTO_RTL); if (res != TRANSITION_DENIED) { break; // changed successfully or already in this state } - print_reject_mode(status, "AUTO_RTL"); + print_reject_mode(status_local, "AUTO_RTL"); // fallback to LOITER if home position not set - res = main_state_transition(status, MAIN_STATE_AUTO_LOITER); + res = main_state_transition(status_local, MAIN_STATE_AUTO_LOITER); if (res != TRANSITION_DENIED) { break; // changed successfully or already in this state } } else if (sp_man->loiter_switch == SWITCH_POS_ON) { - res = main_state_transition(status, MAIN_STATE_AUTO_LOITER); + res = main_state_transition(status_local, MAIN_STATE_AUTO_LOITER); if (res != TRANSITION_DENIED) { break; // changed successfully or already in this state } - print_reject_mode(status, "AUTO_LOITER"); + print_reject_mode(status_local, "AUTO_LOITER"); } else { - res = main_state_transition(status, MAIN_STATE_AUTO_MISSION); + res = main_state_transition(status_local, MAIN_STATE_AUTO_MISSION); if (res != TRANSITION_DENIED) { break; // changed successfully or already in this state } - print_reject_mode(status, "AUTO_MISSION"); + print_reject_mode(status_local, "AUTO_MISSION"); + + // fallback to LOITER if home position not set + res = main_state_transition(status_local, MAIN_STATE_AUTO_LOITER); + + if (res != TRANSITION_DENIED) { + break; // changed successfully or already in this state + } } // fallback to POSCTL - res = main_state_transition(status, MAIN_STATE_POSCTL); + res = main_state_transition(status_local, MAIN_STATE_POSCTL); if (res != TRANSITION_DENIED) { break; // changed successfully or already in this state } // fallback to ALTCTL - res = main_state_transition(status, MAIN_STATE_ALTCTL); + res = main_state_transition(status_local, MAIN_STATE_ALTCTL); if (res != TRANSITION_DENIED) { break; // changed successfully or already in this state } // fallback to MANUAL - res = main_state_transition(status, MAIN_STATE_MANUAL); + res = main_state_transition(status_local, MAIN_STATE_MANUAL); // TRANSITION_DENIED is not possible here break; @@ -1768,6 +1889,7 @@ set_control_mode() /* TODO: check this */ control_mode.flag_external_manual_override_ok = !status.is_rotary_wing; control_mode.flag_system_hil_enabled = status.hil_state == HIL_STATE_ON; + control_mode.flag_control_offboard_enabled = false; switch (status.nav_state) { case NAVIGATION_STATE_MANUAL: @@ -1806,6 +1928,54 @@ set_control_mode() control_mode.flag_control_termination_enabled = false; break; + case NAVIGATION_STATE_OFFBOARD: + control_mode.flag_control_manual_enabled = false; + control_mode.flag_control_auto_enabled = false; + control_mode.flag_control_offboard_enabled = true; + + switch (sp_offboard.mode) { + case OFFBOARD_CONTROL_MODE_DIRECT_RATES: + control_mode.flag_control_rates_enabled = true; + control_mode.flag_control_attitude_enabled = false; + control_mode.flag_control_altitude_enabled = false; + control_mode.flag_control_climb_rate_enabled = false; + control_mode.flag_control_position_enabled = false; + control_mode.flag_control_velocity_enabled = false; + break; + case OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE: + control_mode.flag_control_rates_enabled = true; + control_mode.flag_control_attitude_enabled = true; + control_mode.flag_control_altitude_enabled = false; + control_mode.flag_control_climb_rate_enabled = false; + control_mode.flag_control_position_enabled = false; + control_mode.flag_control_velocity_enabled = false; + break; + case OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY: + control_mode.flag_control_rates_enabled = true; + control_mode.flag_control_attitude_enabled = true; + control_mode.flag_control_altitude_enabled = true; /* XXX: hack for now */ + control_mode.flag_control_climb_rate_enabled = true; + control_mode.flag_control_position_enabled = true; /* XXX: hack for now */ + control_mode.flag_control_velocity_enabled = true; + break; + case OFFBOARD_CONTROL_MODE_DIRECT_POSITION: + control_mode.flag_control_rates_enabled = true; + control_mode.flag_control_attitude_enabled = true; + control_mode.flag_control_altitude_enabled = true; + control_mode.flag_control_climb_rate_enabled = true; + control_mode.flag_control_position_enabled = true; + control_mode.flag_control_velocity_enabled = true; + break; + default: + control_mode.flag_control_rates_enabled = false; + control_mode.flag_control_attitude_enabled = false; + control_mode.flag_control_altitude_enabled = false; + control_mode.flag_control_climb_rate_enabled = false; + control_mode.flag_control_position_enabled = false; + control_mode.flag_control_velocity_enabled = false; + } + break; + case NAVIGATION_STATE_POSCTL: control_mode.flag_control_manual_enabled = true; control_mode.flag_control_auto_enabled = false; @@ -1865,15 +2035,13 @@ set_control_mode() } void -print_reject_mode(struct vehicle_status_s *status, const char *msg) +print_reject_mode(struct vehicle_status_s *status_local, const char *msg) { hrt_abstime t = hrt_absolute_time(); if (t - last_print_mode_reject_time > PRINT_MODE_REJECT_INTERVAL) { last_print_mode_reject_time = t; - char s[80]; - sprintf(s, "#audio: REJECT %s", msg); - mavlink_log_critical(mavlink_fd, s); + mavlink_log_critical(mavlink_fd, "REJECT %s", msg); /* only buzz if armed, because else we're driving people nuts indoors they really need to look at the leds as well. */ @@ -1888,9 +2056,7 @@ print_reject_arm(const char *msg) if (t - last_print_mode_reject_time > PRINT_MODE_REJECT_INTERVAL) { last_print_mode_reject_time = t; - char s[80]; - sprintf(s, "#audio: %s", msg); - mavlink_log_critical(mavlink_fd, s); + mavlink_log_critical(mavlink_fd, msg); tune_negative(true); } } @@ -1903,12 +2069,12 @@ void answer_command(struct vehicle_command_s &cmd, enum VEHICLE_CMD_RESULT resul break; case VEHICLE_CMD_RESULT_DENIED: - mavlink_log_critical(mavlink_fd, "#audio: command denied: %u", cmd.command); + mavlink_log_critical(mavlink_fd, "command denied: %u", cmd.command); tune_negative(true); break; case VEHICLE_CMD_RESULT_FAILED: - mavlink_log_critical(mavlink_fd, "#audio: command failed: %u", cmd.command); + mavlink_log_critical(mavlink_fd, "command failed: %u", cmd.command); tune_negative(true); break; @@ -1919,7 +2085,7 @@ void answer_command(struct vehicle_command_s &cmd, enum VEHICLE_CMD_RESULT resul break; case VEHICLE_CMD_RESULT_UNSUPPORTED: - mavlink_log_critical(mavlink_fd, "#audio: command unsupported: %u", cmd.command); + mavlink_log_critical(mavlink_fd, "command unsupported: %u", cmd.command); tune_negative(true); break; @@ -2004,7 +2170,7 @@ void *commander_low_prio_loop(void *arg) int calib_ret = ERROR; /* try to go to INIT/PREFLIGHT arming state */ - if (TRANSITION_DENIED == arming_state_transition(&status, &safety, ARMING_STATE_INIT, &armed, mavlink_fd)) { + if (TRANSITION_DENIED == arming_state_transition(&status, &safety, ARMING_STATE_INIT, &armed, true /* fRunPreArmChecks */, mavlink_fd)) { answer_command(cmd, VEHICLE_CMD_RESULT_DENIED); break; } @@ -2067,7 +2233,7 @@ void *commander_low_prio_loop(void *arg) tune_negative(true); } - arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed, mavlink_fd); + arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed, true /* fRunPreArmChecks */, mavlink_fd); break; } diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp index 80e6861f6..2022e99fb 100644 --- a/src/modules/commander/commander_helper.cpp +++ b/src/modules/commander/commander_helper.cpp @@ -1,9 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. - * Author: Thomas Gubler <thomasgubler@student.ethz.ch> - * Julian Oes <joes@student.ethz.ch> - * Anton Babushkin <anton.babushkin@me.com> + * 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 @@ -37,6 +34,11 @@ /** * @file commander_helper.cpp * Commander helper functions implementations + * + * @author Thomas Gubler <thomasgubler@student.ethz.ch> + * @author Julian Oes <julian@oes.ch> + * @author Anton Babushkin <anton.babushkin@me.com> + * */ #include <stdio.h> @@ -279,15 +281,17 @@ void rgbled_set_pattern(rgbled_pattern_t *pattern) } } -float battery_remaining_estimate_voltage(float voltage, float discharged) +float battery_remaining_estimate_voltage(float voltage, float discharged, float throttle_normalized) { float ret = 0; static param_t bat_v_empty_h; static param_t bat_v_full_h; static param_t bat_n_cells_h; static param_t bat_capacity_h; - static float bat_v_empty = 3.2f; - static float bat_v_full = 4.0f; + static param_t bat_v_load_drop_h; + static float bat_v_empty = 3.4f; + static float bat_v_full = 4.2f; + static float bat_v_load_drop = 0.06f; static int bat_n_cells = 3; static float bat_capacity = -1.0f; static bool initialized = false; @@ -295,23 +299,26 @@ float battery_remaining_estimate_voltage(float voltage, float discharged) if (!initialized) { bat_v_empty_h = param_find("BAT_V_EMPTY"); - bat_v_full_h = param_find("BAT_V_FULL"); + bat_v_full_h = param_find("BAT_V_CHARGED"); bat_n_cells_h = param_find("BAT_N_CELLS"); bat_capacity_h = param_find("BAT_CAPACITY"); + bat_v_load_drop_h = param_find("BAT_V_LOAD_DROP"); initialized = true; } if (counter % 100 == 0) { param_get(bat_v_empty_h, &bat_v_empty); param_get(bat_v_full_h, &bat_v_full); + param_get(bat_v_load_drop_h, &bat_v_load_drop); param_get(bat_n_cells_h, &bat_n_cells); param_get(bat_capacity_h, &bat_capacity); } counter++; - /* remaining charge estimate based on voltage */ - float remaining_voltage = (voltage - bat_n_cells * bat_v_empty) / (bat_n_cells * (bat_v_full - bat_v_empty)); + /* remaining charge estimate based on voltage and internal resistance (drop under load) */ + float bat_v_full_dynamic = bat_v_full - (bat_v_load_drop * throttle_normalized); + float remaining_voltage = (voltage - (bat_n_cells * bat_v_empty)) / (bat_n_cells * (bat_v_full_dynamic - bat_v_empty)); if (bat_capacity > 0.0f) { /* if battery capacity is known, use discharged current for estimate, but don't show more than voltage estimate */ diff --git a/src/modules/commander/commander_helper.h b/src/modules/commander/commander_helper.h index e75f2592f..4a77fe487 100644 --- a/src/modules/commander/commander_helper.h +++ b/src/modules/commander/commander_helper.h @@ -1,8 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. - * Author: Thomas Gubler <thomasgubler@student.ethz.ch> - * Julian Oes <joes@student.ethz.ch> + * 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 @@ -36,6 +34,9 @@ /** * @file commander_helper.h * Commander helper functions definitions + * + * @author Thomas Gubler <thomasgubler@student.ethz.ch> + * @author Julian Oes <julian@oes.ch> */ #ifndef COMMANDER_HELPER_H_ @@ -77,8 +78,11 @@ void rgbled_set_pattern(rgbled_pattern_t *pattern); * Use integral of current if battery capacity known (BAT_CAPACITY parameter set), * else use simple estimate based on voltage. * + * @param voltage the current battery voltage + * @param discharged the discharged capacity + * @param throttle_normalized the normalized throttle magnitude from 0 to 1. Negative throttle should be converted to this range as well, as it consumes energy. * @return the estimated remaining capacity in 0..1 */ -float battery_remaining_estimate_voltage(float voltage, float discharged); +float battery_remaining_estimate_voltage(float voltage, float discharged, float throttle_normalized); #endif /* COMMANDER_HELPER_H_ */ diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index 4750f9d5c..dba68700b 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -65,7 +65,17 @@ PARAM_DEFINE_FLOAT(BAT_V_EMPTY, 3.4f); * * @group Battery Calibration */ -PARAM_DEFINE_FLOAT(BAT_V_FULL, 3.9f); +PARAM_DEFINE_FLOAT(BAT_V_CHARGED, 4.2f); + +/** + * Voltage drop per cell on 100% load + * + * This implicitely defines the internal resistance + * to maximum current ratio and assumes linearity. + * + * @group Battery Calibration + */ +PARAM_DEFINE_FLOAT(BAT_V_LOAD_DROP, 0.07f); /** * Number of cells. diff --git a/src/modules/commander/commander_tests/state_machine_helper_test.cpp b/src/modules/commander/commander_tests/state_machine_helper_test.cpp index ee0d08391..08dda2fab 100644 --- a/src/modules/commander/commander_tests/state_machine_helper_test.cpp +++ b/src/modules/commander/commander_tests/state_machine_helper_test.cpp @@ -183,12 +183,12 @@ bool StateMachineHelperTest::armingStateTransitionTest(void) // Safety switch arming tests - { "transition: init to standby, no safety switch", + { "transition: standby to armed, no safety switch", { ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_NOT_AVAILABLE, ATT_SAFETY_OFF, ARMING_STATE_ARMED, { ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED }, - { "transition: init to standby, safety switch off", + { "transition: standby to armed, safety switch off", { ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_OFF, ARMING_STATE_ARMED, { ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED }, @@ -286,7 +286,7 @@ bool StateMachineHelperTest::armingStateTransitionTest(void) armed.ready_to_arm = test->current_state.ready_to_arm; // Attempt transition - transition_result_t result = arming_state_transition(&status, &safety, test->requested_state, &armed); + transition_result_t result = arming_state_transition(&status, &safety, test->requested_state, &armed, false /* no pre-arm checks */, 0 /* no mavlink_fd */); // Validate result of transition ut_assert(test->assertMsg, test->expected_transition_result == result); @@ -300,70 +300,151 @@ bool StateMachineHelperTest::armingStateTransitionTest(void) bool StateMachineHelperTest::mainStateTransitionTest(void) { - struct vehicle_status_s current_state; - main_state_t new_main_state; + // This structure represent a single test case for testing Main State transitions. + typedef struct { + const char* assertMsg; // Text to show when test case fails + uint8_t condition_bits; // Bits for various condition_* values + main_state_t from_state; // State prior to transition request + main_state_t to_state; // State to transition to + transition_result_t expected_transition_result; // Expected result from main_state_transition call + } MainTransitionTest_t; + + // Bits for condition_bits + #define MTT_ALL_NOT_VALID 0 + #define MTT_ROTARY_WING 1 << 0 + #define MTT_LOC_ALT_VALID 1 << 1 + #define MTT_LOC_POS_VALID 1 << 2 + #define MTT_HOME_POS_VALID 1 << 3 + #define MTT_GLOBAL_POS_VALID 1 << 4 + + static const MainTransitionTest_t rgMainTransitionTests[] = { + + // TRANSITION_NOT_CHANGED tests + + { "no transition: identical states", + MTT_ALL_NOT_VALID, + MAIN_STATE_MANUAL, MAIN_STATE_MANUAL, TRANSITION_NOT_CHANGED }, + + // TRANSITION_CHANGED tests + + { "transition: MANUAL to ACRO", + MTT_ALL_NOT_VALID, + MAIN_STATE_MANUAL, MAIN_STATE_ACRO, TRANSITION_CHANGED }, + + { "transition: ACRO to MANUAL", + MTT_ALL_NOT_VALID, + MAIN_STATE_ACRO, MAIN_STATE_MANUAL, TRANSITION_CHANGED }, + + { "transition: MANUAL to AUTO_MISSION - global position valid, home position valid", + MTT_GLOBAL_POS_VALID | MTT_HOME_POS_VALID, + MAIN_STATE_MANUAL, MAIN_STATE_AUTO_MISSION, TRANSITION_CHANGED }, + + { "transition: AUTO_MISSION to MANUAL - global position valid, home position valid", + MTT_GLOBAL_POS_VALID | MTT_HOME_POS_VALID, + MAIN_STATE_AUTO_MISSION, MAIN_STATE_MANUAL, TRANSITION_CHANGED }, + + { "transition: MANUAL to AUTO_LOITER - global position valid", + MTT_GLOBAL_POS_VALID, + MAIN_STATE_MANUAL, MAIN_STATE_AUTO_LOITER, TRANSITION_CHANGED }, + + { "transition: AUTO_LOITER to MANUAL - global position valid", + MTT_GLOBAL_POS_VALID, + MAIN_STATE_AUTO_LOITER, MAIN_STATE_MANUAL, TRANSITION_CHANGED }, + + { "transition: MANUAL to AUTO_RTL - global position valid, home position valid", + MTT_GLOBAL_POS_VALID | MTT_HOME_POS_VALID, + MAIN_STATE_MANUAL, MAIN_STATE_AUTO_RTL, TRANSITION_CHANGED }, + + { "transition: AUTO_RTL to MANUAL - global position valid, home position valid", + MTT_GLOBAL_POS_VALID | MTT_HOME_POS_VALID, + MAIN_STATE_AUTO_RTL, MAIN_STATE_MANUAL, TRANSITION_CHANGED }, + + { "transition: MANUAL to ALTCTL - not rotary", + MTT_ALL_NOT_VALID, + MAIN_STATE_MANUAL, MAIN_STATE_ALTCTL, TRANSITION_CHANGED }, + + { "transition: MANUAL to ALTCTL - rotary, global position not valid, local altitude valid", + MTT_ROTARY_WING | MTT_LOC_ALT_VALID, + MAIN_STATE_MANUAL, MAIN_STATE_ALTCTL, TRANSITION_CHANGED }, + + { "transition: MANUAL to ALTCTL - rotary, global position valid, local altitude not valid", + MTT_ROTARY_WING | MTT_GLOBAL_POS_VALID, + MAIN_STATE_MANUAL, MAIN_STATE_ALTCTL, TRANSITION_CHANGED }, + + { "transition: ALTCTL to MANUAL - local altitude valid", + MTT_LOC_ALT_VALID, + MAIN_STATE_ALTCTL, MAIN_STATE_MANUAL, TRANSITION_CHANGED }, + + { "transition: MANUAL to POSCTL - local position not valid, global position valid", + MTT_GLOBAL_POS_VALID, + MAIN_STATE_MANUAL, MAIN_STATE_POSCTL, TRANSITION_CHANGED }, + + { "transition: MANUAL to POSCTL - local position valid, global position not valid", + MTT_LOC_POS_VALID, + MAIN_STATE_MANUAL, MAIN_STATE_POSCTL, TRANSITION_CHANGED }, + + { "transition: POSCTL to MANUAL - local position valid, global position valid", + MTT_LOC_POS_VALID, + MAIN_STATE_POSCTL, MAIN_STATE_MANUAL, TRANSITION_CHANGED }, + + // TRANSITION_DENIED tests + + { "no transition: MANUAL to AUTO_MISSION - global position not valid", + MTT_ALL_NOT_VALID, + MAIN_STATE_MANUAL, MAIN_STATE_AUTO_MISSION, TRANSITION_DENIED }, + + { "no transition: MANUAL to AUTO_LOITER - global position not valid", + MTT_ALL_NOT_VALID, + MAIN_STATE_MANUAL, MAIN_STATE_AUTO_LOITER, TRANSITION_DENIED }, + + { "no transition: MANUAL to AUTO_RTL - global position not valid, home position not valid", + MTT_ALL_NOT_VALID, + MAIN_STATE_MANUAL, MAIN_STATE_AUTO_RTL, TRANSITION_DENIED }, + + { "no transition: MANUAL to AUTO_RTL - global position not valid, home position valid", + MTT_HOME_POS_VALID, + MAIN_STATE_MANUAL, MAIN_STATE_AUTO_RTL, TRANSITION_DENIED }, + + { "no transition: MANUAL to AUTO_RTL - global position valid, home position not valid", + MTT_GLOBAL_POS_VALID, + MAIN_STATE_MANUAL, MAIN_STATE_AUTO_RTL, TRANSITION_DENIED }, + + { "no transition: MANUAL to ALTCTL - rotary, global position not valid, local altitude not valid", + MTT_ROTARY_WING, + MAIN_STATE_MANUAL, MAIN_STATE_ALTCTL, TRANSITION_DENIED }, + + { "no transition: MANUAL to POSCTL - local position not valid, global position not valid", + MTT_ALL_NOT_VALID, + MAIN_STATE_MANUAL, MAIN_STATE_POSCTL, TRANSITION_DENIED }, + }; - // Identical states. - current_state.main_state = MAIN_STATE_MANUAL; - new_main_state = MAIN_STATE_MANUAL; - ut_assert("no transition: identical states", - TRANSITION_NOT_CHANGED == main_state_transition(¤t_state, new_main_state)); - ut_assert("current state: manual", MAIN_STATE_MANUAL == current_state.main_state); - - // AUTO to MANUAL. - current_state.main_state = MAIN_STATE_AUTO; - new_main_state = MAIN_STATE_MANUAL; - ut_assert("transition changed: auto to manual", - TRANSITION_CHANGED == main_state_transition(¤t_state, new_main_state)); - ut_assert("new state: manual", MAIN_STATE_MANUAL == current_state.main_state); - - // MANUAL to ALTCTRL. - current_state.main_state = MAIN_STATE_MANUAL; - current_state.condition_local_altitude_valid = true; - new_main_state = MAIN_STATE_ALTCTL; - ut_assert("tranisition: manual to altctrl", - TRANSITION_CHANGED == main_state_transition(¤t_state, new_main_state)); - ut_assert("new state: altctrl", MAIN_STATE_ALTCTL == current_state.main_state); - - // MANUAL to ALTCTRL, invalid local altitude. - current_state.main_state = MAIN_STATE_MANUAL; - current_state.condition_local_altitude_valid = false; - new_main_state = MAIN_STATE_ALTCTL; - ut_assert("no transition: invalid local altitude", - TRANSITION_DENIED == main_state_transition(¤t_state, new_main_state)); - ut_assert("current state: manual", MAIN_STATE_MANUAL == current_state.main_state); - - // MANUAL to POSCTRL. - current_state.main_state = MAIN_STATE_MANUAL; - current_state.condition_local_position_valid = true; - new_main_state = MAIN_STATE_POSCTL; - ut_assert("transition: manual to posctrl", - TRANSITION_CHANGED == main_state_transition(¤t_state, new_main_state)); - ut_assert("current state: posctrl", MAIN_STATE_POSCTL == current_state.main_state); - - // MANUAL to POSCTRL, invalid local position. - current_state.main_state = MAIN_STATE_MANUAL; - current_state.condition_local_position_valid = false; - new_main_state = MAIN_STATE_POSCTL; - ut_assert("no transition: invalid position", - TRANSITION_DENIED == main_state_transition(¤t_state, new_main_state)); - ut_assert("current state: manual", MAIN_STATE_MANUAL == current_state.main_state); - - // MANUAL to AUTO. - current_state.main_state = MAIN_STATE_MANUAL; - current_state.condition_global_position_valid = true; - new_main_state = MAIN_STATE_AUTO; - ut_assert("transition: manual to auto", - TRANSITION_CHANGED == main_state_transition(¤t_state, new_main_state)); - ut_assert("current state: auto", MAIN_STATE_AUTO == current_state.main_state); - - // MANUAL to AUTO, invalid global position. - current_state.main_state = MAIN_STATE_MANUAL; - current_state.condition_global_position_valid = false; - new_main_state = MAIN_STATE_AUTO; - ut_assert("no transition: invalid global position", - TRANSITION_DENIED == main_state_transition(¤t_state, new_main_state)); - ut_assert("current state: manual", MAIN_STATE_MANUAL == current_state.main_state); + size_t cMainTransitionTests = sizeof(rgMainTransitionTests) / sizeof(rgMainTransitionTests[0]); + for (size_t i=0; i<cMainTransitionTests; i++) { + const MainTransitionTest_t* test = &rgMainTransitionTests[i]; + + // Setup initial machine state + struct vehicle_status_s current_state; + current_state.main_state = test->from_state; + current_state.is_rotary_wing = test->condition_bits & MTT_ROTARY_WING; + current_state.condition_local_altitude_valid = test->condition_bits & MTT_LOC_ALT_VALID; + current_state.condition_local_position_valid = test->condition_bits & MTT_LOC_POS_VALID; + current_state.condition_home_position_valid = test->condition_bits & MTT_HOME_POS_VALID; + current_state.condition_global_position_valid = test->condition_bits & MTT_GLOBAL_POS_VALID; + + // Attempt transition + transition_result_t result = main_state_transition(¤t_state, test->to_state); + + // Validate result of transition + ut_assert(test->assertMsg, test->expected_transition_result == result); + if (test->expected_transition_result == result) { + if (test->expected_transition_result == TRANSITION_CHANGED) { + ut_assert(test->assertMsg, test->to_state == current_state.main_state); + } else { + ut_assert(test->assertMsg, test->from_state == current_state.main_state); + } + } + } + return true; } diff --git a/src/modules/commander/gyro_calibration.cpp b/src/modules/commander/gyro_calibration.cpp index cbc2844c1..d89c67c2b 100644 --- a/src/modules/commander/gyro_calibration.cpp +++ b/src/modules/commander/gyro_calibration.cpp @@ -92,7 +92,7 @@ int do_gyro_calibration(int mavlink_fd) unsigned poll_errcount = 0; /* subscribe to gyro sensor topic */ - int sub_sensor_gyro = orb_subscribe(ORB_ID(sensor_gyro)); + int sub_sensor_gyro = orb_subscribe(ORB_ID(sensor_gyro0)); struct gyro_report gyro_report; while (calibration_counter < calibration_count) { @@ -104,7 +104,7 @@ int do_gyro_calibration(int mavlink_fd) int poll_ret = poll(fds, 1, 1000); if (poll_ret > 0) { - orb_copy(ORB_ID(sensor_gyro), sub_sensor_gyro, &gyro_report); + orb_copy(ORB_ID(sensor_gyro0), sub_sensor_gyro, &gyro_report); gyro_scale.x_offset += gyro_report.x; gyro_scale.y_offset += gyro_report.y; gyro_scale.z_offset += gyro_report.z; diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp index 0ead22f77..23900f386 100644 --- a/src/modules/commander/mag_calibration.cpp +++ b/src/modules/commander/mag_calibration.cpp @@ -145,7 +145,7 @@ int do_mag_calibration(int mavlink_fd) } if (res == OK) { - int sub_mag = orb_subscribe(ORB_ID(sensor_mag)); + int sub_mag = orb_subscribe(ORB_ID(sensor_mag0)); struct mag_report mag; /* limit update rate to get equally spaced measurements over time (in ms) */ @@ -170,7 +170,7 @@ int do_mag_calibration(int mavlink_fd) int poll_ret = poll(fds, 1, 1000); if (poll_ret > 0) { - orb_copy(ORB_ID(sensor_mag), sub_mag, &mag); + orb_copy(ORB_ID(sensor_mag0), sub_mag, &mag); x[calibration_counter] = mag.x; y[calibration_counter] = mag.y; diff --git a/src/modules/commander/mag_calibration.h b/src/modules/commander/mag_calibration.h index a101cd7b1..c14f948cc 100644 --- a/src/modules/commander/mag_calibration.h +++ b/src/modules/commander/mag_calibration.h @@ -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 diff --git a/src/modules/commander/px4_custom_mode.h b/src/modules/commander/px4_custom_mode.h index 7f5f93801..eaf309288 100644 --- a/src/modules/commander/px4_custom_mode.h +++ b/src/modules/commander/px4_custom_mode.h @@ -16,6 +16,7 @@ enum PX4_CUSTOM_MAIN_MODE { PX4_CUSTOM_MAIN_MODE_POSCTL, PX4_CUSTOM_MAIN_MODE_AUTO, PX4_CUSTOM_MAIN_MODE_ACRO, + PX4_CUSTOM_MAIN_MODE_OFFBOARD, }; enum PX4_CUSTOM_SUB_MODE_AUTO { diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 244513375..f8589d24b 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -52,6 +52,7 @@ #include <uORB/topics/vehicle_status.h> #include <uORB/topics/actuator_controls.h> #include <uORB/topics/differential_pressure.h> +#include <uORB/topics/airspeed.h> #include <systemlib/systemlib.h> #include <systemlib/param/param.h> #include <systemlib/err.h> @@ -70,8 +71,6 @@ #endif static const int ERROR = -1; -static int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd); - // This array defines the arming state transitions. The rows are the new state, and the columns // are the current state. Using new state and current state you can index into the array which // will be true for a valid transition or false for a invalid transition. In some cases even @@ -89,7 +88,7 @@ static const bool arming_transitions[ARMING_STATE_MAX][ARMING_STATE_MAX] = { }; // You can index into the array with an arming_state_t in order to get it's textual representation -static const char *state_names[ARMING_STATE_MAX] = { +static const char * const state_names[ARMING_STATE_MAX] = { "ARMING_STATE_INIT", "ARMING_STATE_STANDBY", "ARMING_STATE_ARMED", @@ -100,19 +99,20 @@ static const char *state_names[ARMING_STATE_MAX] = { }; transition_result_t -arming_state_transition(struct vehicle_status_s *status, /// current vehicle status - const struct safety_s *safety, /// current safety settings - arming_state_t new_arming_state, /// arming state requested - struct actuator_armed_s *armed, /// current armed status - const int mavlink_fd) /// mavlink fd for error reporting, 0 for none +arming_state_transition(struct vehicle_status_s *status, ///< current vehicle status + const struct safety_s *safety, ///< current safety settings + arming_state_t new_arming_state, ///< arming state requested + struct actuator_armed_s *armed, ///< current armed status + bool fRunPreArmChecks, ///< true: run the pre-arm checks, false: no pre-arm checks, for unit testing + const int mavlink_fd) ///< mavlink fd for error reporting, 0 for none { // Double check that our static arrays are still valid ASSERT(ARMING_STATE_INIT == 0); ASSERT(ARMING_STATE_IN_AIR_RESTORE == ARMING_STATE_MAX - 1); transition_result_t ret = TRANSITION_DENIED; - arming_state_t current_arming_state = status->arming_state; + bool feedback_provided = false; /* only check transition if the new state is actually different from the current one */ if (new_arming_state == current_arming_state) { @@ -126,7 +126,7 @@ arming_state_transition(struct vehicle_status_s *status, /// current int prearm_ret = OK; /* only perform the check if we have to */ - if (new_arming_state == ARMING_STATE_ARMED) { + if (fRunPreArmChecks && new_arming_state == ARMING_STATE_ARMED) { prearm_ret = prearm_check(status, mavlink_fd); } @@ -157,13 +157,15 @@ arming_state_transition(struct vehicle_status_s *status, /// current // Fail transition if pre-arm check fails if (prearm_ret) { + /* the prearm check already prints the reject reason */ + feedback_provided = true; valid_transition = false; // Fail transition if we need safety switch press } else if (safety->safety_switch_available && !safety->safety_off) { - mavlink_log_critical(mavlink_fd, "#audio: NOT ARMING: Press safety switch!"); - + mavlink_log_critical(mavlink_fd, "NOT ARMING: Press safety switch first!"); + feedback_provided = true; valid_transition = false; } @@ -173,16 +175,18 @@ arming_state_transition(struct vehicle_status_s *status, /// current // Fail transition if power is not good if (!status->condition_power_input_valid) { - mavlink_log_critical(mavlink_fd, "#audio: NOT ARMING: Connect power module."); + mavlink_log_critical(mavlink_fd, "NOT ARMING: Connect power module."); + feedback_provided = true; valid_transition = false; } // Fail transition if power levels on the avionics rail // are measured but are insufficient - if (status->condition_power_input_valid && (status->avionics_power_rail_voltage > 0.0f) && - (status->avionics_power_rail_voltage < 4.9f)) { + if (status->condition_power_input_valid && ((status->avionics_power_rail_voltage > 0.0f) && + (status->avionics_power_rail_voltage < 4.9f))) { - mavlink_log_critical(mavlink_fd, "#audio: NOT ARMING: Avionics power low: %6.2f V.", status->avionics_power_rail_voltage); + mavlink_log_critical(mavlink_fd, "NOT ARMING: Avionics power low: %6.2f V.", (double)status->avionics_power_rail_voltage); + feedback_provided = true; valid_transition = false; } } @@ -201,6 +205,8 @@ arming_state_transition(struct vehicle_status_s *status, /// current /* Sensors need to be initialized for STANDBY state */ if (new_arming_state == ARMING_STATE_STANDBY && !status->condition_system_sensors_initialized) { + mavlink_log_critical(mavlink_fd, "NOT ARMING: Sensors not operational."); + feedback_provided = true; valid_transition = false; } @@ -217,11 +223,14 @@ arming_state_transition(struct vehicle_status_s *status, /// current } if (ret == TRANSITION_DENIED) { - static const char *errMsg = "INVAL: %s - %s"; + const char * str = "INVAL: %s - %s"; + /* only print to console here by default as this is too technical to be useful during operation */ + warnx(str, state_names[status->arming_state], state_names[new_arming_state]); - mavlink_log_critical(mavlink_fd, errMsg, state_names[status->arming_state], state_names[new_arming_state]); - - warnx(errMsg, state_names[status->arming_state], state_names[new_arming_state]); + /* print to MAVLink if we didn't provide any feedback yet */ + if (!feedback_provided) { + mavlink_log_critical(mavlink_fd, str, state_names[status->arming_state], state_names[new_arming_state]); + } } return ret; @@ -271,7 +280,6 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta } break; - case MAIN_STATE_AUTO_MISSION: case MAIN_STATE_AUTO_LOITER: /* need global position estimate */ if (status->condition_global_position_valid) { @@ -279,6 +287,7 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta } break; + case MAIN_STATE_AUTO_MISSION: case MAIN_STATE_AUTO_RTL: /* need global position and home position */ if (status->condition_global_position_valid && status->condition_home_position_valid) { @@ -286,6 +295,15 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta } break; + case MAIN_STATE_OFFBOARD: + + /* need offboard signal */ + if (!status->offboard_control_signal_lost) { + ret = TRANSITION_CHANGED; + } + + break; + case MAIN_STATE_MAX: default: break; @@ -584,6 +602,25 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en } break; + case MAIN_STATE_OFFBOARD: + /* require offboard control, otherwise stay where you are */ + if (status->offboard_control_signal_lost && !status->rc_signal_lost) { + status->failsafe = true; + + status->nav_state = NAVIGATION_STATE_POSCTL; + } else if (status->offboard_control_signal_lost && status->rc_signal_lost) { + status->failsafe = true; + + if (status->condition_local_position_valid) { + status->nav_state = NAVIGATION_STATE_LAND; + } else if (status->condition_local_altitude_valid) { + status->nav_state = NAVIGATION_STATE_DESCEND; + } else { + status->nav_state = NAVIGATION_STATE_TERMINATION; + } + } else { + status->nav_state = NAVIGATION_STATE_OFFBOARD; + } default: break; } @@ -594,19 +631,21 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd) { int ret; + bool failed = false; int fd = open(ACCEL_DEVICE_PATH, O_RDONLY); if (fd < 0) { - mavlink_log_critical(mavlink_fd, "#audio: FAIL: ACCEL SENSOR MISSING"); - ret = fd; + mavlink_log_critical(mavlink_fd, "ARM FAIL: ACCEL SENSOR MISSING"); + failed = true; goto system_eval; } ret = ioctl(fd, ACCELIOCSELFTEST, 0); if (ret != OK) { - mavlink_log_critical(mavlink_fd, "#audio: FAIL: ACCEL CALIBRATION"); + mavlink_log_critical(mavlink_fd, "ARM FAIL: ACCEL CALIBRATION"); + failed = true; goto system_eval; } @@ -616,55 +655,44 @@ int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd) if (ret == sizeof(acc)) { /* evaluate values */ - float accel_scale = sqrtf(acc.x * acc.x + acc.y * acc.y + acc.z * acc.z); - - if (accel_scale < 9.78f || accel_scale > 9.83f) { - mavlink_log_info(mavlink_fd, "#audio: Accelerometer calibration recommended."); - } + float accel_magnitude = sqrtf(acc.x * acc.x + acc.y * acc.y + acc.z * acc.z); - if (accel_scale > 30.0f /* m/s^2 */) { - mavlink_log_critical(mavlink_fd, "#audio: FAIL: ACCEL RANGE"); + if (accel_magnitude < 4.0f || accel_magnitude > 15.0f /* m/s^2 */) { + mavlink_log_critical(mavlink_fd, "ARM FAIL: ACCEL RANGE, hold still"); /* this is frickin' fatal */ - ret = ERROR; + failed = true; goto system_eval; - } else { - ret = OK; } } else { - mavlink_log_critical(mavlink_fd, "#audio: FAIL: ACCEL READ"); + mavlink_log_critical(mavlink_fd, "ARM FAIL: ACCEL READ"); /* this is frickin' fatal */ - ret = ERROR; + failed = true; goto system_eval; } - if (!status->is_rotary_wing) { - int fd = open(AIRSPEED_DEVICE_PATH, O_RDONLY); + /* Perform airspeed check only if circuit breaker is not + * engaged and it's not a rotary wing */ + if (!status->circuit_breaker_engaged_airspd_check && !status->is_rotary_wing) { + /* accel done, close it */ + close(fd); + fd = orb_subscribe(ORB_ID(airspeed)); + + struct airspeed_s airspeed; - if (fd < 0) { - mavlink_log_critical(mavlink_fd, "#audio: FAIL: AIRSPEED SENSOR MISSING"); - ret = fd; + if ((ret = orb_copy(ORB_ID(airspeed), fd, &airspeed)) || + (hrt_elapsed_time(&airspeed.timestamp) > (50 * 1000))) { + mavlink_log_critical(mavlink_fd, "ARM FAIL: AIRSPEED SENSOR MISSING"); + failed = true; goto system_eval; } - struct differential_pressure_s diff_pres; - - ret = read(fd, &diff_pres, sizeof(diff_pres)); - - if (ret == sizeof(diff_pres)) { - if (fabsf(diff_pres.differential_pressure_filtered_pa > 5.0f)) { - mavlink_log_critical(mavlink_fd, "#audio: WARNING AIRSPEED CALIBRATION MISSING"); - // XXX do not make this fatal yet - ret = OK; - } - } else { - mavlink_log_critical(mavlink_fd, "#audio: FAIL: AIRSPEED READ"); - /* this is frickin' fatal */ - ret = ERROR; - goto system_eval; + if (fabsf(airspeed.indicated_airspeed_m_s > 6.0f)) { + mavlink_log_critical(mavlink_fd, "AIRSPEED WARNING: WIND OR CALIBRATION MISSING"); + // XXX do not make this fatal yet } } system_eval: close(fd); - return ret; + return (failed); } diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h index 11072403e..69ce8bbce 100644 --- a/src/modules/commander/state_machine_helper.h +++ b/src/modules/commander/state_machine_helper.h @@ -1,8 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. - * Author: Thomas Gubler <thomasgubler@student.ethz.ch> - * Julian Oes <joes@student.ethz.ch> + * 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 @@ -36,14 +34,14 @@ /** * @file state_machine_helper.h * State machine helper functions definitions + * + * @author Thomas Gubler <thomasgubler@student.ethz.ch> + * @author Julian Oes <julian@oes.ch> */ #ifndef STATE_MACHINE_HELPER_H_ #define STATE_MACHINE_HELPER_H_ -#define GPS_NOFIX_COUNTER_LIMIT 4 //need GPS_NOFIX_COUNTER_LIMIT gps packets with a bad fix to call an error (if outdoor) -#define GPS_GOTFIX_COUNTER_REQUIRED 4 //need GPS_GOTFIX_COUNTER_REQUIRED gps packets with a good fix to obtain position lock - #include <uORB/uORB.h> #include <uORB/topics/vehicle_status.h> #include <uORB/topics/actuator_armed.h> @@ -59,7 +57,7 @@ typedef enum { bool is_safe(const struct vehicle_status_s *current_state, const struct safety_s *safety, const struct actuator_armed_s *armed); transition_result_t arming_state_transition(struct vehicle_status_s *current_state, const struct safety_s *safety, - arming_state_t new_arming_state, struct actuator_armed_s *armed, const int mavlink_fd); + arming_state_t new_arming_state, struct actuator_armed_s *armed, bool fRunPreArmChecks, const int mavlink_fd); transition_result_t main_state_transition(struct vehicle_status_s *current_state, main_state_t new_main_state); @@ -67,4 +65,6 @@ transition_result_t hil_state_transition(hil_state_t new_state, int status_pub, bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_enabled, const bool mission_finished); +int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd); + #endif /* STATE_MACHINE_HELPER_H_ */ diff --git a/src/modules/controllib/block/Block.hpp b/src/modules/controllib/block/Block.hpp index 736698e21..9bd80b15b 100644 --- a/src/modules/controllib/block/Block.hpp +++ b/src/modules/controllib/block/Block.hpp @@ -93,6 +93,11 @@ protected: List<uORB::SubscriptionBase *> _subscriptions; List<uORB::PublicationBase *> _publications; List<BlockParamBase *> _params; + +private: + /* this class has pointer data members and should not be copied (private constructor) */ + Block(const control::Block&); + Block operator=(const control::Block&); }; class __EXPORT SuperBlock : diff --git a/src/modules/controllib/blocks.cpp b/src/modules/controllib/blocks.cpp index c6c374300..f739446fa 100644 --- a/src/modules/controllib/blocks.cpp +++ b/src/modules/controllib/blocks.cpp @@ -121,6 +121,9 @@ int blockLimitSymTest() float BlockLowPass::update(float input) { + if (!isfinite(getState())) { + setState(input); + } float b = 2 * float(M_PI) * getFCut() * getDt(); float a = b / (1 + b); setState(a * input + (1 - a)*getState()); @@ -293,7 +296,18 @@ int blockIntegralTrapTest() float BlockDerivative::update(float input) { - float output = _lowPass.update((input - getU()) / getDt()); + float output; + if (_initialized) { + output = _lowPass.update((input - getU()) / getDt()); + } else { + // if this is the first call to update + // we have no valid derivative + // and so we use the assumption the + // input value is not changing much, + // which is the best we can do here. + output = 0.0f; + _initialized = true; + } setU(input); return output; } diff --git a/src/modules/controllib/blocks.hpp b/src/modules/controllib/blocks.hpp index 66e929038..bffc355a8 100644 --- a/src/modules/controllib/blocks.hpp +++ b/src/modules/controllib/blocks.hpp @@ -114,7 +114,7 @@ public: // methods BlockLowPass(SuperBlock *parent, const char *name) : Block(parent, name), - _state(0), + _state(0.0f/0.0f /* initialize to invalid val, force into is_finite() check on first call */), _fCut(this, "") // only one parameter, no need to name {}; virtual ~BlockLowPass() {}; @@ -238,9 +238,25 @@ public: BlockDerivative(SuperBlock *parent, const char *name) : SuperBlock(parent, name), _u(0), + _initialized(false), _lowPass(this, "LP") {}; virtual ~BlockDerivative() {}; + + /** + * Update the state and get current derivative + * + * This call updates the state and gets the current + * derivative. As the derivative is only valid + * on the second call to update, it will return + * no change (0) on the first. To get a closer + * estimate of the derivative on the first call, + * call setU() one time step before using the + * return value of update(). + * + * @param input the variable to calculate the derivative of + * @return the current derivative + */ float update(float input); // accessors void setU(float u) {_u = u;} @@ -249,6 +265,7 @@ public: protected: // attributes float _u; /**< previous input */ + bool _initialized; BlockLowPass _lowPass; /**< low pass filter */ }; diff --git a/src/modules/dataman/dataman.c b/src/modules/dataman/dataman.c index 406293bda..ca1fe9bbb 100644 --- a/src/modules/dataman/dataman.c +++ b/src/modules/dataman/dataman.c @@ -62,6 +62,8 @@ __EXPORT int dataman_main(int argc, char *argv[]); __EXPORT ssize_t dm_read(dm_item_t item, unsigned char index, void *buffer, size_t buflen); __EXPORT ssize_t dm_write(dm_item_t item, unsigned char index, dm_persitence_t persistence, const void *buffer, size_t buflen); __EXPORT int dm_clear(dm_item_t item); +__EXPORT void dm_lock(dm_item_t item); +__EXPORT void dm_unlock(dm_item_t item); __EXPORT int dm_restart(dm_reset_reason restart_type); /** Types of function calls supported by the worker task */ @@ -114,12 +116,17 @@ static const unsigned g_per_item_max_index[DM_KEY_NUM_KEYS] = { DM_KEY_FENCE_POINTS_MAX, DM_KEY_WAYPOINTS_OFFBOARD_0_MAX, DM_KEY_WAYPOINTS_OFFBOARD_1_MAX, - DM_KEY_WAYPOINTS_ONBOARD_MAX + DM_KEY_WAYPOINTS_ONBOARD_MAX, + DM_KEY_MISSION_STATE_MAX }; /* Table of offset for index 0 of each item type */ static unsigned int g_key_offsets[DM_KEY_NUM_KEYS]; +/* Item type lock mutexes */ +static sem_t *g_item_locks[DM_KEY_NUM_KEYS]; +static sem_t g_sys_state_mutex; + /* The data manager store file handle and file name */ static int g_fd = -1, g_task_fd = -1; static const char *k_data_manager_device_path = "/fs/microsd/dataman"; @@ -139,22 +146,22 @@ static work_q_t g_work_q; /* pending work items. To be consumed by worker thread 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 */ +static bool g_task_should_exit; /**< if true, dataman task should exit */ #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)); /* Initialize the NuttX queue structure */ + 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 */ + q->size = q->max_size = 0; /* Queue is initially empty */ } static inline void destroy_q(work_q_t *q) { - sem_destroy(&(q->mutex)); /* Destroy the queue lock */ + sem_destroy(&(q->mutex)); /* Destroy the queue lock */ } static inline void @@ -318,7 +325,9 @@ _write(dm_item_t item, unsigned char index, dm_persitence_t persistence, const v buffer[1] = persistence; buffer[2] = 0; buffer[3] = 0; - memcpy(buffer + DM_SECTOR_HDR_SIZE, buf, count); + if (count > 0) { + memcpy(buffer + DM_SECTOR_HDR_SIZE, buf, count); + } count += DM_SECTOR_HDR_SIZE; len = -1; @@ -568,6 +577,32 @@ dm_clear(dm_item_t item) return enqueue_work_item_and_wait_for_result(work); } +__EXPORT void +dm_lock(dm_item_t item) +{ + /* Make sure data manager has been started and is not shutting down */ + if ((g_fd < 0) || g_task_should_exit) + return; + if (item >= DM_KEY_NUM_KEYS) + return; + if (g_item_locks[item]) { + sem_wait(g_item_locks[item]); + } +} + +__EXPORT void +dm_unlock(dm_item_t item) +{ + /* Make sure data manager has been started and is not shutting down */ + if ((g_fd < 0) || g_task_should_exit) + return; + if (item >= DM_KEY_NUM_KEYS) + return; + if (g_item_locks[item]) { + sem_post(g_item_locks[item]); + } +} + /* Tell the data manager about the type of the last reset */ __EXPORT int dm_restart(dm_reset_reason reason) @@ -608,6 +643,12 @@ task_main(int argc, char *argv[]) for (unsigned i = 0; i < dm_number_of_funcs; i++) g_func_counts[i] = 0; + /* Initialize the item type locks, for now only DM_KEY_MISSION_STATE supports locking */ + sem_init(&g_sys_state_mutex, 1, 1); /* Initially unlocked */ + for (unsigned i = 0; i < DM_KEY_NUM_KEYS; i++) + g_item_locks[i] = NULL; + g_item_locks[DM_KEY_MISSION_STATE] = &g_sys_state_mutex; + g_task_should_exit = false; init_q(&g_work_q); @@ -743,6 +784,7 @@ task_main(int argc, char *argv[]) destroy_q(&g_work_q); destroy_q(&g_free_q); sem_destroy(&g_work_queued_sema); + sem_destroy(&g_sys_state_mutex); return 0; } diff --git a/src/modules/dataman/dataman.h b/src/modules/dataman/dataman.h index 1dfb26f73..d625bf985 100644 --- a/src/modules/dataman/dataman.h +++ b/src/modules/dataman/dataman.h @@ -53,16 +53,20 @@ extern "C" { DM_KEY_WAYPOINTS_OFFBOARD_0, /* Mission way point coordinates sent over mavlink */ DM_KEY_WAYPOINTS_OFFBOARD_1, /* (alernate between 0 and 1) */ DM_KEY_WAYPOINTS_ONBOARD, /* Mission way point coordinates generated onboard */ + DM_KEY_MISSION_STATE, /* Persistent mission state */ DM_KEY_NUM_KEYS /* Total number of item types defined */ } dm_item_t; + #define DM_KEY_WAYPOINTS_OFFBOARD(_id) (_id == 0 ? DM_KEY_WAYPOINTS_OFFBOARD_0 : DM_KEY_WAYPOINTS_OFFBOARD_1) + /** The maximum number of instances for each item type */ enum { DM_KEY_SAFE_POINTS_MAX = 8, DM_KEY_FENCE_POINTS_MAX = GEOFENCE_MAX_VERTICES, DM_KEY_WAYPOINTS_OFFBOARD_0_MAX = NUM_MISSIONS_SUPPORTED, DM_KEY_WAYPOINTS_OFFBOARD_1_MAX = NUM_MISSIONS_SUPPORTED, - DM_KEY_WAYPOINTS_ONBOARD_MAX = NUM_MISSIONS_SUPPORTED + DM_KEY_WAYPOINTS_ONBOARD_MAX = NUM_MISSIONS_SUPPORTED, + DM_KEY_MISSION_STATE_MAX = 1 }; /** Data persistence levels */ @@ -101,6 +105,18 @@ extern "C" { size_t buflen /* Length in bytes of data to retrieve */ ); + /** Lock all items of this type */ + __EXPORT void + dm_lock( + dm_item_t item /* The item type to clear */ + ); + + /** Unlock all items of this type */ + __EXPORT void + dm_unlock( + dm_item_t item /* The item type to clear */ + ); + /** Erase all items of this type */ __EXPORT int dm_clear( diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index e4f805dc0..91d17e787 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -112,6 +112,10 @@ public: */ FixedwingEstimator(); + /* we do not want people ever copying this class */ + FixedwingEstimator(const FixedwingEstimator& that) = delete; + FixedwingEstimator operator=(const FixedwingEstimator&) = delete; + /** * Destructor, also kills the sensors task. */ @@ -125,6 +129,13 @@ public: int start(); /** + * Task status + * + * @return true if the mainloop is running + */ + bool task_running() { return _task_running; } + + /** * Print the current status. */ void print_status(); @@ -151,7 +162,8 @@ public: private: bool _task_should_exit; /**< if true, sensor task should exit */ - int _estimator_task; /**< task handle for sensor task */ + bool _task_running; /**< if true, task is running in its mainloop */ + int _estimator_task; /**< task handle for sensor task */ #ifndef SENSOR_COMBINED_SUB int _gyro_sub; /**< gyro sensor subscription */ int _accel_sub; /**< accel sensor subscription */ @@ -313,12 +325,13 @@ namespace estimator #endif static const int ERROR = -1; -FixedwingEstimator *g_estimator; +FixedwingEstimator *g_estimator = nullptr; } FixedwingEstimator::FixedwingEstimator() : _task_should_exit(false), + _task_running(false), _estimator_task(-1), /* subscriptions */ @@ -362,9 +375,10 @@ FixedwingEstimator::FixedwingEstimator() : _mag_offsets({}), #ifdef SENSOR_COMBINED_SUB - _sensor_combined({}), + _sensor_combined{}, #endif + _pos_ref{}, _baro_ref(0.0f), _baro_ref_offset(0.0f), _baro_gps_offset(0.0f), @@ -381,12 +395,18 @@ FixedwingEstimator::FixedwingEstimator() : /* states */ _baro_init(false), _gps_initialized(false), + _gps_start_time(0), + _filter_start_time(0), + _last_sensor_timestamp(0), + _last_run(0), _gyro_valid(false), _accel_valid(false), _mag_valid(false), _ekf_logging(true), _debug(0), _mavlink_fd(-1), + _parameters{}, + _parameter_handles{}, _ekf(nullptr), _velocity_xy_filtered(0.0f), _velocity_z_filtered(0.0f), @@ -559,61 +579,26 @@ FixedwingEstimator::check_filter_state() int check = _ekf->CheckAndBound(&ekf_report); - const char* ekfname = "att pos estimator: "; - - switch (check) { - case 0: - /* all ok */ - break; - case 1: - { - const char* str = "NaN in states, resetting"; - warnx("%s", str); - mavlink_log_critical(_mavlink_fd, "%s%s", ekfname, str); - break; - } - case 2: - { - const char* str = "stale IMU data, resetting"; - warnx("%s", str); - mavlink_log_critical(_mavlink_fd, "%s%s", ekfname, str); - break; - } - case 3: - { - const char* str = "switching to dynamic state"; - warnx("%s", str); - mavlink_log_info(_mavlink_fd, "%s%s", ekfname, str); - break; - } - case 4: - { - const char* str = "excessive gyro offsets"; - warnx("%s", str); - mavlink_log_info(_mavlink_fd, "%s%s", ekfname, str); - break; - } - case 5: - { - const char* str = "GPS velocity divergence"; - warnx("%s", str); - mavlink_log_info(_mavlink_fd, "%s%s", ekfname, str); - break; - } - case 6: - { - const char* str = "excessive covariances"; - warnx("%s", str); - mavlink_log_info(_mavlink_fd, "%s%s", ekfname, str); - break; + const char* const feedback[] = { 0, + "NaN in states, resetting", + "stale IMU data, resetting", + "got initial position lock", + "excessive gyro offsets", + "GPS velocity divergence", + "excessive covariances", + "unknown condition"}; + + // Print out error condition + if (check) { + unsigned warn_index = static_cast<unsigned>(check); + unsigned max_warn_index = (sizeof(feedback) / sizeof(feedback[0])); + + if (max_warn_index < warn_index) { + warn_index = max_warn_index; } - default: - { - const char* str = "unknown reset condition"; - warnx("%s", str); - mavlink_log_critical(_mavlink_fd, "%s%s", ekfname, str); - } + warnx("reset: %s", feedback[warn_index]); + mavlink_log_critical(_mavlink_fd, "[ekf] re-init: %s", feedback[warn_index]); } struct estimator_status_report rep; @@ -645,6 +630,10 @@ FixedwingEstimator::check_filter_state() rep.health_flags |= (((uint8_t)ekf_report.posHealth) << 1); rep.health_flags |= (((uint8_t)ekf_report.hgtHealth) << 2); rep.health_flags |= (((uint8_t)!ekf_report.gyroOffsetsExcessive) << 3); + rep.health_flags |= (((uint8_t)ekf_report.onGround) << 4); + rep.health_flags |= (((uint8_t)ekf_report.staticMode) << 5); + rep.health_flags |= (((uint8_t)ekf_report.useCompass) << 6); + rep.health_flags |= (((uint8_t)ekf_report.useAirspeed) << 7); rep.timeout_flags |= (((uint8_t)ekf_report.velTimeout) << 0); rep.timeout_flags |= (((uint8_t)ekf_report.posTimeout) << 1); @@ -715,7 +704,7 @@ FixedwingEstimator::task_main() /* * do subscriptions */ - _baro_sub = orb_subscribe(ORB_ID(sensor_baro)); + _baro_sub = orb_subscribe(ORB_ID(sensor_baro0)); _airspeed_sub = orb_subscribe(ORB_ID(airspeed)); _gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); _vstatus_sub = orb_subscribe(ORB_ID(vehicle_status)); @@ -743,8 +732,8 @@ FixedwingEstimator::task_main() /* sets also parameters in the EKF object */ parameters_update(); - Vector3f lastAngRate = {0.0f, 0.0f, 0.0f}; - Vector3f lastAccel = {0.0f, 0.0f, 0.0f}; + Vector3f lastAngRate; + Vector3f lastAccel; /* wakeup source(s) */ struct pollfd fds[2]; @@ -772,6 +761,8 @@ FixedwingEstimator::task_main() _gps.vel_e_m_s = 0.0f; _gps.vel_d_m_s = 0.0f; + _task_running = true; + while (!_task_should_exit) { /* wait for up to 500ms for data */ @@ -1061,7 +1052,7 @@ FixedwingEstimator::task_main() orb_check(_baro_sub, &baro_updated); if (baro_updated) { - orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro); + orb_copy(ORB_ID(sensor_baro0), _baro_sub, &_baro); _ekf->baroHgt = _baro.altitude; @@ -1153,7 +1144,7 @@ FixedwingEstimator::task_main() initVelNED[2] = _gps.vel_d_m_s; // Set up height correctly - orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro); + orb_copy(ORB_ID(sensor_baro0), _baro_sub, &_baro); _baro_ref_offset = _ekf->states[9]; // this should become zero in the local frame _baro_gps_offset = _baro.altitude - gps_alt; _ekf->baroHgt = _baro.altitude; @@ -1202,10 +1193,10 @@ FixedwingEstimator::task_main() _baro_gps_offset = 0.0f; _ekf->InitialiseFilter(initVelNED, 0.0, 0.0, 0.0f, 0.0f); + } else if (_ekf->statesInitialised) { // We're apparently initialized in this case now - int check = check_filter_state(); if (check) { @@ -1213,7 +1204,6 @@ FixedwingEstimator::task_main() continue; } - // Run the strapdown INS equations every IMU update _ekf->UpdateStrapdownEquationsNED(); #if 0 @@ -1281,7 +1271,11 @@ FixedwingEstimator::task_main() // run the fusion step _ekf->FuseVelposNED(); - } else if (_ekf->statesInitialised) { + } else if (!_gps_initialized) { + + // force static mode + _ekf->staticMode = true; + // Convert GPS measurements to Pos NE, hgt and Vel NED _ekf->velNED[0] = 0.0f; _ekf->velNED[1] = 0.0f; @@ -1303,7 +1297,7 @@ FixedwingEstimator::task_main() _ekf->fusePosData = false; } - if (newHgtData && _ekf->statesInitialised) { + if (newHgtData) { // Could use a blend of GPS and baro alt data if desired _ekf->hgtMea = 1.0f * (_ekf->baroHgt - _baro_ref); _ekf->fuseHgtData = true; @@ -1317,7 +1311,7 @@ FixedwingEstimator::task_main() } // Fuse Magnetometer Measurements - if (newDataMag && _ekf->statesInitialised) { + if (newDataMag) { _ekf->fuseMagData = true; _ekf->RecallStates(_ekf->statesAtMagMeasTime, (IMUmsec - _parameters.mag_delay_ms)); // Assume 50 msec avg delay for magnetometer data @@ -1331,7 +1325,7 @@ FixedwingEstimator::task_main() } // Fuse Airspeed Measurements - if (newAdsData && _ekf->statesInitialised && _ekf->VtasMeas > 8.0f) { + if (newAdsData && _ekf->VtasMeas > 7.0f) { _ekf->fuseVtasData = true; _ekf->RecallStates(_ekf->statesAtVtasMeasTime, (IMUmsec - _parameters.tas_delay_ms)); // assume 100 msec avg delay for airspeed data _ekf->FuseAirspeed(); @@ -1399,7 +1393,7 @@ FixedwingEstimator::task_main() _velocity_xy_filtered = 0.95f*_velocity_xy_filtered + 0.05f*sqrtf(_local_pos.vx*_local_pos.vx + _local_pos.vy*_local_pos.vy); _velocity_z_filtered = 0.95f*_velocity_z_filtered + 0.05f*fabsf(_local_pos.vz); - _airspeed_filtered = 0.95f*_airspeed_filtered + + 0.05f*_airspeed.true_airspeed_m_s; + _airspeed_filtered = 0.95f*_airspeed_filtered + 0.05f*_airspeed.true_airspeed_m_s; /* crude land detector for fixedwing only, @@ -1475,8 +1469,8 @@ FixedwingEstimator::task_main() _wind.timestamp = _global_pos.timestamp; _wind.windspeed_north = _ekf->states[14]; _wind.windspeed_east = _ekf->states[15]; - _wind.covariance_north = 0.0f; // XXX get form filter - _wind.covariance_east = 0.0f; + _wind.covariance_north = _ekf->P[14][14]; + _wind.covariance_east = _ekf->P[15][15]; /* lazily publish the wind estimate only once available */ if (_wind_pub > 0) { @@ -1487,30 +1481,12 @@ FixedwingEstimator::task_main() /* advertise and publish */ _wind_pub = orb_advertise(ORB_ID(wind_estimate), &_wind); } - } } } - if (hrt_elapsed_time(&_wind.timestamp) > 99000) { - _wind.timestamp = _global_pos.timestamp; - _wind.windspeed_north = _ekf->states[14]; - _wind.windspeed_east = _ekf->states[15]; - _wind.covariance_north = _ekf->P[14][14]; - _wind.covariance_east = _ekf->P[15][15]; - - /* lazily publish the wind estimate only once available */ - if (_wind_pub > 0) { - /* publish the wind estimate */ - orb_publish(ORB_ID(wind_estimate), _wind_pub, &_wind); - - } else { - /* advertise and publish */ - _wind_pub = orb_advertise(ORB_ID(wind_estimate), &_wind); - } - } } } @@ -1518,6 +1494,8 @@ FixedwingEstimator::task_main() perf_end(_loop_perf); } + _task_running = false; + warnx("exiting.\n"); _estimator_task = -1; @@ -1533,7 +1511,7 @@ FixedwingEstimator::start() _estimator_task = task_spawn_cmd("ekf_att_pos_estimator", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 40, - 6000, + 5000, (main_t)&FixedwingEstimator::task_main_trampoline, nullptr); @@ -1670,6 +1648,14 @@ int ekf_att_pos_estimator_main(int argc, char *argv[]) err(1, "start failed"); } + /* avoid memory fragmentation by not exiting start handler until the task has fully started */ + while (estimator::g_estimator == nullptr || !estimator::g_estimator->task_running()) { + usleep(50000); + printf("."); + fflush(stdout); + } + printf("\n"); + exit(0); } diff --git a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp index 9622f7e40..c7c7305b2 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp @@ -5,46 +5,120 @@ #define EKF_COVARIANCE_DIVERGED 1.0e8f -AttPosEKF::AttPosEKF() - - /* NOTE: DO NOT initialize class members here. Use ZeroVariables() - * instead to allow clean in-air re-initialization. - */ +AttPosEKF::AttPosEKF() : + covTimeStepMax(0.0f), + covDelAngMax(0.0f), + rngFinderPitch(0.0f), + a1(0.0f), + a2(0.0f), + a3(0.0f), + yawVarScale(0.0f), + windVelSigma(0.0f), + dAngBiasSigma(0.0f), + dVelBiasSigma(0.0f), + magEarthSigma(0.0f), + magBodySigma(0.0f), + gndHgtSigma(0.0f), + vneSigma(0.0f), + vdSigma(0.0f), + posNeSigma(0.0f), + posDSigma(0.0f), + magMeasurementSigma(0.0f), + airspeedMeasurementSigma(0.0f), + gyroProcessNoise(0.0f), + accelProcessNoise(0.0f), + EAS2TAS(1.0f), + magstate{}, + resetMagState{}, + KH{}, + KHP{}, + P{}, + Kfusion{}, + states{}, + resetStates{}, + storedStates{}, + statetimeStamp{}, + statesAtVelTime{}, + statesAtPosTime{}, + statesAtHgtTime{}, + statesAtMagMeasTime{}, + statesAtVtasMeasTime{}, + statesAtRngTime{}, + statesAtOptFlowTime{}, + correctedDelAng(), + correctedDelVel(), + summedDelAng(), + summedDelVel(), + accNavMag(), + earthRateNED(), + angRate(), + lastGyroOffset(), + delAngTotal(), + Tbn(), + Tnb(), + accel(), + dVelIMU(), + dAngIMU(), + dtIMU(0), + fusionModeGPS(0), + innovVelPos{}, + varInnovVelPos{}, + velNED{}, + accelGPSNED{}, + posNE{}, + hgtMea(0.0f), + baroHgtOffset(0.0f), + rngMea(0.0f), + innovMag{}, + varInnovMag{}, + magData{}, + losData{}, + innovVtas(0.0f), + innovRng(0.0f), + innovOptFlow{}, + varInnovOptFlow{}, + varInnovVtas(0.0f), + varInnovRng(0.0f), + VtasMeas(0.0f), + magDeclination(0.0f), + latRef(0.0f), + lonRef(-M_PI_F), + hgtRef(0.0f), + refSet(false), + magBias(), + covSkipCount(0), + gpsLat(0.0), + gpsLon(-M_PI), + gpsHgt(0.0f), + GPSstatus(0), + baroHgt(0.0f), + statesInitialised(false), + fuseVelData(false), + fusePosData(false), + fuseHgtData(false), + fuseMagData(false), + fuseVtasData(false), + fuseRngData(false), + fuseOptFlowData(false), + + inhibitWindStates(true), + inhibitMagStates(true), + inhibitGndHgtState(true), + + onGround(true), + staticMode(true), + useAirspeed(true), + useCompass(true), + useRangeFinder(false), + useOpticalFlow(false), + + ekfDiverged(false), + lastReset(0), + current_ekf_state{}, + last_ekf_error{}, + numericalProtection(true), + storeIndex(0) { - summedDelAng.zero(); - summedDelVel.zero(); - - fusionModeGPS = 0; - fuseVelData = false; - fusePosData = false; - fuseHgtData = false; - fuseMagData = false; - fuseVtasData = false; - onGround = true; - staticMode = true; - useAirspeed = true; - useCompass = true; - useRangeFinder = true; - numericalProtection = true; - refSet = false; - storeIndex = 0; - gpsHgt = 0.0f; - baroHgt = 0.0f; - GPSstatus = 0; - VtasMeas = 0.0f; - magDeclination = 0.0f; - dAngIMU.zero(); - dVelIMU.zero(); - velNED[0] = 0.0f; - velNED[1] = 0.0f; - velNED[2] = 0.0f; - accelGPSNED[0] = 0.0f; - accelGPSNED[1] = 0.0f; - accelGPSNED[2] = 0.0f; - delAngTotal.zero(); - ekfDiverged = false; - lastReset = 0; - memset(&last_ekf_error, 0, sizeof(last_ekf_error)); memset(¤t_ekf_state, 0, sizeof(current_ekf_state)); ZeroVariables(); @@ -72,7 +146,7 @@ void AttPosEKF::UpdateStrapdownEquationsNED() float qUpdated[4]; float quatMag; float deltaQuat[4]; - const Vector3f gravityNED = {0.0,0.0,GRAVITY_MSS}; + const Vector3f gravityNED(0.0f, 0.0f, GRAVITY_MSS); // Remove sensor bias errors correctedDelAng.x = dAngIMU.x - states[10]; @@ -227,10 +301,22 @@ void AttPosEKF::CovariancePrediction(float dt) // scale gyro bias noise when on ground to allow for faster bias estimation for (uint8_t i=10; i<=12; i++) processNoise[i] = dt * dAngBiasSigma; processNoise[13] = dVelBiasSigma; - for (uint8_t i=14; i<=15; i++) processNoise[i] = dt * windVelSigma; - for (uint8_t i=16; i<=18; i++) processNoise[i] = dt * magEarthSigma; - for (uint8_t i=19; i<=21; i++) processNoise[i] = dt * magBodySigma; - processNoise[22] = dt * sqrtf(sq(states[4]) + sq(states[5])) * gndHgtSigma; + if (!inhibitWindStates) { + for (uint8_t i=14; i<=15; i++) processNoise[i] = dt * windVelSigma; + } else { + for (uint8_t i=14; i<=15; i++) processNoise[i] = 0; + } + if (!inhibitMagStates) { + for (uint8_t i=16; i<=18; i++) processNoise[i] = dt * magEarthSigma; + for (uint8_t i=19; i<=21; i++) processNoise[i] = dt * magBodySigma; + } else { + for (uint8_t i=16; i<=21; i++) processNoise[i] = 0; + } + if (!inhibitGndHgtState) { + processNoise[22] = dt * sqrtf(sq(states[4]) + sq(states[5])) * gndHgtSigma; + } else { + processNoise[22] = 0; + } // square all sigmas for (unsigned i = 0; i < n_states; i++) processNoise[i] = sq(processNoise[i]); @@ -842,30 +928,6 @@ void AttPosEKF::CovariancePrediction(float dt) nextP[i][i] = nextP[i][i] + processNoise[i]; } - // If on ground or no magnetometer fitted, inhibit magnetometer bias updates by - // setting the coresponding covariance terms to zero. - if (onGround || !useCompass) - { - zeroRows(nextP,16,21); - zeroCols(nextP,16,21); - } - - // If on ground or not using airspeed sensing, inhibit wind velocity - // covariance growth. - if (onGround || !useAirspeed) - { - zeroRows(nextP,14,15); - zeroCols(nextP,14,15); - } - - // If on ground, inhibit terrain offset updates by - // setting the coresponding covariance terms to zero. - if (onGround) - { - zeroRows(nextP,22,22); - zeroCols(nextP,22,22); - } - // If the total position variance exceds 1E6 (1000m), then stop covariance // growth by setting the predicted to the previous values // This prevent an ill conditioned matrix from occurring for long periods @@ -882,48 +944,22 @@ void AttPosEKF::CovariancePrediction(float dt) } } - if (onGround || staticMode) { - // copy the portion of the variances we want to - // propagate - for (unsigned i = 0; i <= 13; i++) { - P[i][i] = nextP[i][i]; - } - - // force symmetry for observable states - // force zero for non-observable states - for (unsigned i = 1; i < n_states; i++) - { - for (uint8_t j = 0; j < i; j++) - { - if ((i > 13) || (j > 13)) { - P[i][j] = 0.0f; - } else { - P[i][j] = 0.5f * (nextP[i][j] + nextP[j][i]); - } - P[j][i] = P[i][j]; - } - } - - } else { - - // Copy covariance - for (unsigned i = 0; i < n_states; i++) { - P[i][i] = nextP[i][i]; - } + // Copy covariance + for (unsigned i = 0; i < n_states; i++) { + P[i][i] = nextP[i][i]; + } - // force symmetry for observable states - for (unsigned i = 1; i < n_states; i++) + // force symmetry for observable states + for (unsigned i = 1; i < n_states; i++) + { + for (uint8_t j = 0; j < i; j++) { - for (uint8_t j = 0; j < i; j++) - { - P[i][j] = 0.5f * (nextP[i][j] + nextP[j][i]); - P[j][i] = P[i][j]; - } + P[i][j] = 0.5f * (nextP[i][j] + nextP[j][i]); + P[j][i] = P[i][j]; } - } - ConstrainVariances(); + ConstrainVariances(); } void AttPosEKF::FuseVelposNED() @@ -944,7 +980,7 @@ void AttPosEKF::FuseVelposNED() bool fuseData[6] = {false,false,false,false,false,false}; uint8_t stateIndex; uint8_t obsIndex; - uint8_t indexLimit; + uint8_t indexLimit = 22; // declare variables used by state and covariance update calculations float velErr; @@ -981,11 +1017,6 @@ void AttPosEKF::FuseVelposNED() R_OBS[4] = R_OBS[3]; R_OBS[5] = sq(posDSigma) + sq(posErr); - // Set innovation variances to zero default - for (uint8_t i = 0; i<=5; i++) - { - varInnovVelPos[i] = 0.0f; - } // calculate innovations and check GPS data validity using an innovation consistency check if (fuseVelData) { @@ -1001,10 +1032,16 @@ void AttPosEKF::FuseVelposNED() // apply a 5-sigma threshold current_ekf_state.velHealth = (sq(velInnov[0]) + sq(velInnov[1]) + sq(velInnov[2])) < 25.0f * (varInnovVelPos[0] + varInnovVelPos[1] + varInnovVelPos[2]); current_ekf_state.velTimeout = (millis() - current_ekf_state.velFailTime) > horizRetryTime; - if (current_ekf_state.velHealth || current_ekf_state.velTimeout) - { + if (current_ekf_state.velHealth || staticMode) { current_ekf_state.velHealth = true; current_ekf_state.velFailTime = millis(); + } else if (current_ekf_state.velTimeout || !current_ekf_state.posHealth) { + // XXX check + current_ekf_state.velHealth = true; + ResetVelocity(); + ResetStoredStates(); + // do not fuse bad data + fuseVelData = false; } else { @@ -1025,6 +1062,17 @@ void AttPosEKF::FuseVelposNED() { current_ekf_state.posHealth = true; current_ekf_state.posFailTime = millis(); + + if (current_ekf_state.posTimeout) { + ResetPosition(); + + // XXX cross-check the state reset + ResetStoredStates(); + + // do not fuse position data on this time + // step + fusePosData = false; + } } else { @@ -1039,10 +1087,18 @@ void AttPosEKF::FuseVelposNED() // apply a 10-sigma threshold current_ekf_state.hgtHealth = sq(hgtInnov) < 100.0f*varInnovVelPos[5]; current_ekf_state.hgtTimeout = (millis() - current_ekf_state.hgtFailTime) > hgtRetryTime; - if (current_ekf_state.hgtHealth || current_ekf_state.hgtTimeout) + if (current_ekf_state.hgtHealth || current_ekf_state.hgtTimeout || staticMode) { current_ekf_state.hgtHealth = true; current_ekf_state.hgtFailTime = millis(); + + // if we just reset from a timeout, do not fuse + // the height data, but reset height and stored states + if (current_ekf_state.hgtTimeout) { + ResetHeight(); + ResetStoredStates(); + fuseHgtData = false; + } } else { @@ -1071,15 +1127,6 @@ void AttPosEKF::FuseVelposNED() { fuseData[5] = true; } - // Limit range of states modified when on ground - if(!onGround) - { - indexLimit = 22; - } - else - { - indexLimit = 13; - } // Fuse measurements sequentially for (obsIndex=0; obsIndex<=5; obsIndex++) { @@ -1113,6 +1160,22 @@ void AttPosEKF::FuseVelposNED() if (obsIndex != 5) { Kfusion[13] = 0; } + // Don't update wind states if inhibited + if (inhibitWindStates) { + Kfusion[14] = 0; + Kfusion[15] = 0; + } + // Don't update magnetic field states if inhibited + if (inhibitMagStates) { + for (uint8_t i = 16; i<=21; i++) + { + Kfusion[i] = 0; + } + } + // Don't update terrain state if inhibited + if (inhibitGndHgtState) { + Kfusion[22] = 0; + } // Calculate state corrections and re-normalise the quaternions for (uint8_t i = 0; i<=indexLimit; i++) @@ -1179,7 +1242,6 @@ void AttPosEKF::FuseMagnetometer() for (uint8_t i = 0; i < n_states; i++) { H_MAG[i] = 0.0f; } - unsigned indexLimit; // Perform sequential fusion of Magnetometer measurements. // This assumes that the errors in the different components are @@ -1189,19 +1251,6 @@ void AttPosEKF::FuseMagnetometer() // associated with sequential fusion if (useCompass && fuseMagData && (obsIndex < 3)) { - // Limit range of states modified when on ground - if(!onGround) - { - indexLimit = n_states; - } - else - { - indexLimit = 13 + 1; - } - - // Sequential fusion of XYZ components to spread processing load across - // three prediction time steps. - // Calculate observation jacobians and Kalman gains if (obsIndex == 0) { @@ -1287,15 +1336,31 @@ void AttPosEKF::FuseMagnetometer() Kfusion[12] = SK_MX[0]*(P[12][19] + P[12][1]*SH_MAG[0] + P[12][3]*SH_MAG[2] + P[12][0]*SK_MX[3] - P[12][2]*SK_MX[2] - P[12][16]*SK_MX[1] + P[12][17]*SK_MX[5] - P[12][18]*SK_MX[4]); // Only height measurements are allowed to modify the Z delta velocity bias state. This improves the stability of the estimate Kfusion[13] = 0.0f;//SK_MX[0]*(P[13][19] + P[13][1]*SH_MAG[0] + P[13][3]*SH_MAG[2] + P[13][0]*SK_MX[3] - P[13][2]*SK_MX[2] - P[13][16]*SK_MX[1] + P[13][17]*SK_MX[5] - P[13][18]*SK_MX[4]); - Kfusion[14] = SK_MX[0]*(P[14][19] + P[14][1]*SH_MAG[0] + P[14][3]*SH_MAG[2] + P[14][0]*SK_MX[3] - P[14][2]*SK_MX[2] - P[14][16]*SK_MX[1] + P[14][17]*SK_MX[5] - P[14][18]*SK_MX[4]); - Kfusion[15] = SK_MX[0]*(P[15][19] + P[15][1]*SH_MAG[0] + P[15][3]*SH_MAG[2] + P[15][0]*SK_MX[3] - P[15][2]*SK_MX[2] - P[15][16]*SK_MX[1] + P[15][17]*SK_MX[5] - P[15][18]*SK_MX[4]); - Kfusion[16] = SK_MX[0]*(P[16][19] + P[16][1]*SH_MAG[0] + P[16][3]*SH_MAG[2] + P[16][0]*SK_MX[3] - P[16][2]*SK_MX[2] - P[16][16]*SK_MX[1] + P[16][17]*SK_MX[5] - P[16][18]*SK_MX[4]); - Kfusion[17] = SK_MX[0]*(P[17][19] + P[17][1]*SH_MAG[0] + P[17][3]*SH_MAG[2] + P[17][0]*SK_MX[3] - P[17][2]*SK_MX[2] - P[17][16]*SK_MX[1] + P[17][17]*SK_MX[5] - P[17][18]*SK_MX[4]); - Kfusion[18] = SK_MX[0]*(P[18][19] + P[18][1]*SH_MAG[0] + P[18][3]*SH_MAG[2] + P[18][0]*SK_MX[3] - P[18][2]*SK_MX[2] - P[18][16]*SK_MX[1] + P[18][17]*SK_MX[5] - P[18][18]*SK_MX[4]); - Kfusion[19] = SK_MX[0]*(P[19][19] + P[19][1]*SH_MAG[0] + P[19][3]*SH_MAG[2] + P[19][0]*SK_MX[3] - P[19][2]*SK_MX[2] - P[19][16]*SK_MX[1] + P[19][17]*SK_MX[5] - P[19][18]*SK_MX[4]); - Kfusion[20] = SK_MX[0]*(P[20][19] + P[20][1]*SH_MAG[0] + P[20][3]*SH_MAG[2] + P[20][0]*SK_MX[3] - P[20][2]*SK_MX[2] - P[20][16]*SK_MX[1] + P[20][17]*SK_MX[5] - P[20][18]*SK_MX[4]); - Kfusion[21] = SK_MX[0]*(P[21][19] + P[21][1]*SH_MAG[0] + P[21][3]*SH_MAG[2] + P[21][0]*SK_MX[3] - P[21][2]*SK_MX[2] - P[21][16]*SK_MX[1] + P[21][17]*SK_MX[5] - P[21][18]*SK_MX[4]); - Kfusion[22] = SK_MX[0]*(P[22][19] + P[22][1]*SH_MAG[0] + P[22][3]*SH_MAG[2] + P[22][0]*SK_MX[3] - P[22][2]*SK_MX[2] - P[22][16]*SK_MX[1] + P[22][17]*SK_MX[5] - P[22][18]*SK_MX[4]); + // Estimation of selected states is inhibited by setting their Kalman gains to zero + if (!inhibitWindStates) { + Kfusion[14] = SK_MX[0]*(P[14][19] + P[14][1]*SH_MAG[0] + P[14][3]*SH_MAG[2] + P[14][0]*SK_MX[3] - P[14][2]*SK_MX[2] - P[14][16]*SK_MX[1] + P[14][17]*SK_MX[5] - P[14][18]*SK_MX[4]); + Kfusion[15] = SK_MX[0]*(P[15][19] + P[15][1]*SH_MAG[0] + P[15][3]*SH_MAG[2] + P[15][0]*SK_MX[3] - P[15][2]*SK_MX[2] - P[15][16]*SK_MX[1] + P[15][17]*SK_MX[5] - P[15][18]*SK_MX[4]); + } else { + Kfusion[14] = 0; + Kfusion[15] = 0; + } + if (!inhibitMagStates) { + Kfusion[16] = SK_MX[0]*(P[16][19] + P[16][1]*SH_MAG[0] + P[16][3]*SH_MAG[2] + P[16][0]*SK_MX[3] - P[16][2]*SK_MX[2] - P[16][16]*SK_MX[1] + P[16][17]*SK_MX[5] - P[16][18]*SK_MX[4]); + Kfusion[17] = SK_MX[0]*(P[17][19] + P[17][1]*SH_MAG[0] + P[17][3]*SH_MAG[2] + P[17][0]*SK_MX[3] - P[17][2]*SK_MX[2] - P[17][16]*SK_MX[1] + P[17][17]*SK_MX[5] - P[17][18]*SK_MX[4]); + Kfusion[18] = SK_MX[0]*(P[18][19] + P[18][1]*SH_MAG[0] + P[18][3]*SH_MAG[2] + P[18][0]*SK_MX[3] - P[18][2]*SK_MX[2] - P[18][16]*SK_MX[1] + P[18][17]*SK_MX[5] - P[18][18]*SK_MX[4]); + Kfusion[19] = SK_MX[0]*(P[19][19] + P[19][1]*SH_MAG[0] + P[19][3]*SH_MAG[2] + P[19][0]*SK_MX[3] - P[19][2]*SK_MX[2] - P[19][16]*SK_MX[1] + P[19][17]*SK_MX[5] - P[19][18]*SK_MX[4]); + Kfusion[20] = SK_MX[0]*(P[20][19] + P[20][1]*SH_MAG[0] + P[20][3]*SH_MAG[2] + P[20][0]*SK_MX[3] - P[20][2]*SK_MX[2] - P[20][16]*SK_MX[1] + P[20][17]*SK_MX[5] - P[20][18]*SK_MX[4]); + Kfusion[21] = SK_MX[0]*(P[21][19] + P[21][1]*SH_MAG[0] + P[21][3]*SH_MAG[2] + P[21][0]*SK_MX[3] - P[21][2]*SK_MX[2] - P[21][16]*SK_MX[1] + P[21][17]*SK_MX[5] - P[21][18]*SK_MX[4]); + } else { + for (uint8_t i=16; i <= 21; i++) { + Kfusion[i] = 0; + } + } + if (!inhibitGndHgtState) { + Kfusion[22] = SK_MX[0]*(P[22][19] + P[22][1]*SH_MAG[0] + P[22][3]*SH_MAG[2] + P[22][0]*SK_MX[3] - P[22][2]*SK_MX[2] - P[22][16]*SK_MX[1] + P[22][17]*SK_MX[5] - P[22][18]*SK_MX[4]); + } else { + Kfusion[22] = 0; + } varInnovMag[0] = 1.0f/SK_MX[0]; innovMag[0] = MagPred[0] - magData.x; } @@ -1342,15 +1407,34 @@ void AttPosEKF::FuseMagnetometer() Kfusion[12] = SK_MY[0]*(P[12][20] + P[12][0]*SH_MAG[2] + P[12][1]*SH_MAG[1] + P[12][2]*SH_MAG[0] - P[12][3]*SK_MY[2] - P[12][17]*SK_MY[1] - P[12][16]*SK_MY[3] + P[12][18]*SK_MY[4]); // Only height measurements are allowed to modify the Z delta velocity bias state. This improves the stability of the estimate Kfusion[13] = 0.0f;//SK_MY[0]*(P[13][20] + P[13][0]*SH_MAG[2] + P[13][1]*SH_MAG[1] + P[13][2]*SH_MAG[0] - P[13][3]*SK_MY[2] - P[13][17]*SK_MY[1] - P[13][16]*SK_MY[3] + P[13][18]*SK_MY[4]); - Kfusion[14] = SK_MY[0]*(P[14][20] + P[14][0]*SH_MAG[2] + P[14][1]*SH_MAG[1] + P[14][2]*SH_MAG[0] - P[14][3]*SK_MY[2] - P[14][17]*SK_MY[1] - P[14][16]*SK_MY[3] + P[14][18]*SK_MY[4]); - Kfusion[15] = SK_MY[0]*(P[15][20] + P[15][0]*SH_MAG[2] + P[15][1]*SH_MAG[1] + P[15][2]*SH_MAG[0] - P[15][3]*SK_MY[2] - P[15][17]*SK_MY[1] - P[15][16]*SK_MY[3] + P[15][18]*SK_MY[4]); - Kfusion[16] = SK_MY[0]*(P[16][20] + P[16][0]*SH_MAG[2] + P[16][1]*SH_MAG[1] + P[16][2]*SH_MAG[0] - P[16][3]*SK_MY[2] - P[16][17]*SK_MY[1] - P[16][16]*SK_MY[3] + P[16][18]*SK_MY[4]); - Kfusion[17] = SK_MY[0]*(P[17][20] + P[17][0]*SH_MAG[2] + P[17][1]*SH_MAG[1] + P[17][2]*SH_MAG[0] - P[17][3]*SK_MY[2] - P[17][17]*SK_MY[1] - P[17][16]*SK_MY[3] + P[17][18]*SK_MY[4]); - Kfusion[18] = SK_MY[0]*(P[18][20] + P[18][0]*SH_MAG[2] + P[18][1]*SH_MAG[1] + P[18][2]*SH_MAG[0] - P[18][3]*SK_MY[2] - P[18][17]*SK_MY[1] - P[18][16]*SK_MY[3] + P[18][18]*SK_MY[4]); - Kfusion[19] = SK_MY[0]*(P[19][20] + P[19][0]*SH_MAG[2] + P[19][1]*SH_MAG[1] + P[19][2]*SH_MAG[0] - P[19][3]*SK_MY[2] - P[19][17]*SK_MY[1] - P[19][16]*SK_MY[3] + P[19][18]*SK_MY[4]); - Kfusion[20] = SK_MY[0]*(P[20][20] + P[20][0]*SH_MAG[2] + P[20][1]*SH_MAG[1] + P[20][2]*SH_MAG[0] - P[20][3]*SK_MY[2] - P[20][17]*SK_MY[1] - P[20][16]*SK_MY[3] + P[20][18]*SK_MY[4]); - Kfusion[21] = SK_MY[0]*(P[21][20] + P[21][0]*SH_MAG[2] + P[21][1]*SH_MAG[1] + P[21][2]*SH_MAG[0] - P[21][3]*SK_MY[2] - P[21][17]*SK_MY[1] - P[21][16]*SK_MY[3] + P[21][18]*SK_MY[4]); - Kfusion[22] = SK_MY[0]*(P[22][20] + P[22][0]*SH_MAG[2] + P[22][1]*SH_MAG[1] + P[22][2]*SH_MAG[0] - P[22][3]*SK_MY[2] - P[22][17]*SK_MY[1] - P[22][16]*SK_MY[3] + P[22][18]*SK_MY[4]); + // Estimation of selected states is inhibited by setting their Kalman gains to zero + if (!inhibitWindStates) { + Kfusion[14] = SK_MY[0]*(P[14][20] + P[14][0]*SH_MAG[2] + P[14][1]*SH_MAG[1] + P[14][2]*SH_MAG[0] - P[14][3]*SK_MY[2] - P[14][17]*SK_MY[1] - P[14][16]*SK_MY[3] + P[14][18]*SK_MY[4]); + Kfusion[15] = SK_MY[0]*(P[15][20] + P[15][0]*SH_MAG[2] + P[15][1]*SH_MAG[1] + P[15][2]*SH_MAG[0] - P[15][3]*SK_MY[2] - P[15][17]*SK_MY[1] - P[15][16]*SK_MY[3] + P[15][18]*SK_MY[4]); + } else { + Kfusion[14] = 0; + Kfusion[15] = 0; + } + if (!inhibitMagStates) { + Kfusion[16] = SK_MY[0]*(P[16][20] + P[16][0]*SH_MAG[2] + P[16][1]*SH_MAG[1] + P[16][2]*SH_MAG[0] - P[16][3]*SK_MY[2] - P[16][17]*SK_MY[1] - P[16][16]*SK_MY[3] + P[16][18]*SK_MY[4]); + Kfusion[17] = SK_MY[0]*(P[17][20] + P[17][0]*SH_MAG[2] + P[17][1]*SH_MAG[1] + P[17][2]*SH_MAG[0] - P[17][3]*SK_MY[2] - P[17][17]*SK_MY[1] - P[17][16]*SK_MY[3] + P[17][18]*SK_MY[4]); + Kfusion[18] = SK_MY[0]*(P[18][20] + P[18][0]*SH_MAG[2] + P[18][1]*SH_MAG[1] + P[18][2]*SH_MAG[0] - P[18][3]*SK_MY[2] - P[18][17]*SK_MY[1] - P[18][16]*SK_MY[3] + P[18][18]*SK_MY[4]); + Kfusion[19] = SK_MY[0]*(P[19][20] + P[19][0]*SH_MAG[2] + P[19][1]*SH_MAG[1] + P[19][2]*SH_MAG[0] - P[19][3]*SK_MY[2] - P[19][17]*SK_MY[1] - P[19][16]*SK_MY[3] + P[19][18]*SK_MY[4]); + Kfusion[20] = SK_MY[0]*(P[20][20] + P[20][0]*SH_MAG[2] + P[20][1]*SH_MAG[1] + P[20][2]*SH_MAG[0] - P[20][3]*SK_MY[2] - P[20][17]*SK_MY[1] - P[20][16]*SK_MY[3] + P[20][18]*SK_MY[4]); + Kfusion[21] = SK_MY[0]*(P[21][20] + P[21][0]*SH_MAG[2] + P[21][1]*SH_MAG[1] + P[21][2]*SH_MAG[0] - P[21][3]*SK_MY[2] - P[21][17]*SK_MY[1] - P[21][16]*SK_MY[3] + P[21][18]*SK_MY[4]); + } else { + Kfusion[16] = 0; + Kfusion[17] = 0; + Kfusion[18] = 0; + Kfusion[19] = 0; + Kfusion[20] = 0; + Kfusion[21] = 0; + } + if (!inhibitGndHgtState) { + Kfusion[22] = SK_MY[0]*(P[22][20] + P[22][0]*SH_MAG[2] + P[22][1]*SH_MAG[1] + P[22][2]*SH_MAG[0] - P[22][3]*SK_MY[2] - P[22][17]*SK_MY[1] - P[22][16]*SK_MY[3] + P[22][18]*SK_MY[4]); + } else { + Kfusion[22] = 0; + } varInnovMag[1] = 1.0f/SK_MY[0]; innovMag[1] = MagPred[1] - magData.y; } @@ -1398,15 +1482,34 @@ void AttPosEKF::FuseMagnetometer() Kfusion[12] = SK_MZ[0]*(P[12][21] + P[12][0]*SH_MAG[1] + P[12][3]*SH_MAG[0] - P[12][1]*SK_MZ[2] + P[12][2]*SK_MZ[3] + P[12][18]*SK_MZ[1] + P[12][16]*SK_MZ[5] - P[12][17]*SK_MZ[4]); // Only height measurements are allowed to modify the Z delta velocity bias state. This improves the stability of the estimate Kfusion[13] = 0.0f;//SK_MZ[0]*(P[13][21] + P[13][0]*SH_MAG[1] + P[13][3]*SH_MAG[0] - P[13][1]*SK_MZ[2] + P[13][2]*SK_MZ[3] + P[13][18]*SK_MZ[1] + P[13][16]*SK_MZ[5] - P[13][17]*SK_MZ[4]); - Kfusion[14] = SK_MZ[0]*(P[14][21] + P[14][0]*SH_MAG[1] + P[14][3]*SH_MAG[0] - P[14][1]*SK_MZ[2] + P[14][2]*SK_MZ[3] + P[14][18]*SK_MZ[1] + P[14][16]*SK_MZ[5] - P[14][17]*SK_MZ[4]); - Kfusion[15] = SK_MZ[0]*(P[15][21] + P[15][0]*SH_MAG[1] + P[15][3]*SH_MAG[0] - P[15][1]*SK_MZ[2] + P[15][2]*SK_MZ[3] + P[15][18]*SK_MZ[1] + P[15][16]*SK_MZ[5] - P[15][17]*SK_MZ[4]); - Kfusion[16] = SK_MZ[0]*(P[16][21] + P[16][0]*SH_MAG[1] + P[16][3]*SH_MAG[0] - P[16][1]*SK_MZ[2] + P[16][2]*SK_MZ[3] + P[16][18]*SK_MZ[1] + P[16][16]*SK_MZ[5] - P[16][17]*SK_MZ[4]); - Kfusion[17] = SK_MZ[0]*(P[17][21] + P[17][0]*SH_MAG[1] + P[17][3]*SH_MAG[0] - P[17][1]*SK_MZ[2] + P[17][2]*SK_MZ[3] + P[17][18]*SK_MZ[1] + P[17][16]*SK_MZ[5] - P[17][17]*SK_MZ[4]); - Kfusion[18] = SK_MZ[0]*(P[18][21] + P[18][0]*SH_MAG[1] + P[18][3]*SH_MAG[0] - P[18][1]*SK_MZ[2] + P[18][2]*SK_MZ[3] + P[18][18]*SK_MZ[1] + P[18][16]*SK_MZ[5] - P[18][17]*SK_MZ[4]); - Kfusion[19] = SK_MZ[0]*(P[19][21] + P[19][0]*SH_MAG[1] + P[19][3]*SH_MAG[0] - P[19][1]*SK_MZ[2] + P[19][2]*SK_MZ[3] + P[19][18]*SK_MZ[1] + P[19][16]*SK_MZ[5] - P[19][17]*SK_MZ[4]); - Kfusion[20] = SK_MZ[0]*(P[20][21] + P[20][0]*SH_MAG[1] + P[20][3]*SH_MAG[0] - P[20][1]*SK_MZ[2] + P[20][2]*SK_MZ[3] + P[20][18]*SK_MZ[1] + P[20][16]*SK_MZ[5] - P[20][17]*SK_MZ[4]); - Kfusion[21] = SK_MZ[0]*(P[21][21] + P[21][0]*SH_MAG[1] + P[21][3]*SH_MAG[0] - P[21][1]*SK_MZ[2] + P[21][2]*SK_MZ[3] + P[21][18]*SK_MZ[1] + P[21][16]*SK_MZ[5] - P[21][17]*SK_MZ[4]); - Kfusion[22] = SK_MZ[0]*(P[22][21] + P[22][0]*SH_MAG[1] + P[22][3]*SH_MAG[0] - P[22][1]*SK_MZ[2] + P[22][2]*SK_MZ[3] + P[22][18]*SK_MZ[1] + P[22][16]*SK_MZ[5] - P[22][17]*SK_MZ[4]); + // Estimation of selected states is inhibited by setting their Kalman gains to zero + if (!inhibitWindStates) { + Kfusion[14] = SK_MZ[0]*(P[14][21] + P[14][0]*SH_MAG[1] + P[14][3]*SH_MAG[0] - P[14][1]*SK_MZ[2] + P[14][2]*SK_MZ[3] + P[14][18]*SK_MZ[1] + P[14][16]*SK_MZ[5] - P[14][17]*SK_MZ[4]); + Kfusion[15] = SK_MZ[0]*(P[15][21] + P[15][0]*SH_MAG[1] + P[15][3]*SH_MAG[0] - P[15][1]*SK_MZ[2] + P[15][2]*SK_MZ[3] + P[15][18]*SK_MZ[1] + P[15][16]*SK_MZ[5] - P[15][17]*SK_MZ[4]); + } else { + Kfusion[14] = 0; + Kfusion[15] = 0; + } + if (!inhibitMagStates) { + Kfusion[16] = SK_MZ[0]*(P[16][21] + P[16][0]*SH_MAG[1] + P[16][3]*SH_MAG[0] - P[16][1]*SK_MZ[2] + P[16][2]*SK_MZ[3] + P[16][18]*SK_MZ[1] + P[16][16]*SK_MZ[5] - P[16][17]*SK_MZ[4]); + Kfusion[17] = SK_MZ[0]*(P[17][21] + P[17][0]*SH_MAG[1] + P[17][3]*SH_MAG[0] - P[17][1]*SK_MZ[2] + P[17][2]*SK_MZ[3] + P[17][18]*SK_MZ[1] + P[17][16]*SK_MZ[5] - P[17][17]*SK_MZ[4]); + Kfusion[18] = SK_MZ[0]*(P[18][21] + P[18][0]*SH_MAG[1] + P[18][3]*SH_MAG[0] - P[18][1]*SK_MZ[2] + P[18][2]*SK_MZ[3] + P[18][18]*SK_MZ[1] + P[18][16]*SK_MZ[5] - P[18][17]*SK_MZ[4]); + Kfusion[19] = SK_MZ[0]*(P[19][21] + P[19][0]*SH_MAG[1] + P[19][3]*SH_MAG[0] - P[19][1]*SK_MZ[2] + P[19][2]*SK_MZ[3] + P[19][18]*SK_MZ[1] + P[19][16]*SK_MZ[5] - P[19][17]*SK_MZ[4]); + Kfusion[20] = SK_MZ[0]*(P[20][21] + P[20][0]*SH_MAG[1] + P[20][3]*SH_MAG[0] - P[20][1]*SK_MZ[2] + P[20][2]*SK_MZ[3] + P[20][18]*SK_MZ[1] + P[20][16]*SK_MZ[5] - P[20][17]*SK_MZ[4]); + Kfusion[21] = SK_MZ[0]*(P[21][21] + P[21][0]*SH_MAG[1] + P[21][3]*SH_MAG[0] - P[21][1]*SK_MZ[2] + P[21][2]*SK_MZ[3] + P[21][18]*SK_MZ[1] + P[21][16]*SK_MZ[5] - P[21][17]*SK_MZ[4]); + } else { + Kfusion[16] = 0; + Kfusion[17] = 0; + Kfusion[18] = 0; + Kfusion[19] = 0; + Kfusion[20] = 0; + Kfusion[21] = 0; + } + if (!inhibitGndHgtState) { + Kfusion[22] = SK_MZ[0]*(P[22][21] + P[22][0]*SH_MAG[1] + P[22][3]*SH_MAG[0] - P[22][1]*SK_MZ[2] + P[22][2]*SK_MZ[3] + P[22][18]*SK_MZ[1] + P[22][16]*SK_MZ[5] - P[22][17]*SK_MZ[4]); + } else { + Kfusion[22] = 0; + } varInnovMag[2] = 1.0f/SK_MZ[0]; innovMag[2] = MagPred[2] - magData.z; @@ -1416,7 +1519,7 @@ void AttPosEKF::FuseMagnetometer() if ((innovMag[obsIndex]*innovMag[obsIndex]/varInnovMag[obsIndex]) < 25.0f) { // correct the state vector - for (uint8_t j= 0; j < indexLimit; j++) + for (uint8_t j= 0; j < n_states; j++) { states[j] = states[j] - Kfusion[j] * innovMag[obsIndex]; } @@ -1433,7 +1536,7 @@ void AttPosEKF::FuseMagnetometer() // correct the covariance P = (I - K*H)*P // take advantage of the empty columns in KH to reduce the // number of operations - for (uint8_t i = 0; i < indexLimit; i++) + for (uint8_t i = 0; i < n_states; i++) { for (uint8_t j = 0; j <= 3; j++) { @@ -1455,9 +1558,9 @@ void AttPosEKF::FuseMagnetometer() } } } - for (uint8_t i = 0; i < indexLimit; i++) + for (uint8_t i = 0; i < n_states; i++) { - for (uint8_t j = 0; j < indexLimit; j++) + for (uint8_t j = 0; j < n_states; j++) { KHP[i][j] = 0.0f; for (uint8_t k = 0; k <= 3; k++) @@ -1474,9 +1577,9 @@ void AttPosEKF::FuseMagnetometer() } } } - for (uint8_t i = 0; i < indexLimit; i++) + for (uint8_t i = 0; i < n_states; i++) { - for (uint8_t j = 0; j < indexLimit; j++) + for (uint8_t j = 0; j < n_states; j++) { P[i][j] = P[i][j] - KHP[i][j]; } @@ -1552,15 +1655,31 @@ void AttPosEKF::FuseAirspeed() Kfusion[12] = SK_TAS*(P[12][4]*SH_TAS[2] - P[12][14]*SH_TAS[2] + P[12][5]*SH_TAS[1] - P[12][15]*SH_TAS[1] + P[12][6]*vd*SH_TAS[0]); // Only height measurements are allowed to modify the Z delta velocity bias state. This improves the stability of the estimate Kfusion[13] = 0.0f;//SK_TAS*(P[13][4]*SH_TAS[2] - P[13][14]*SH_TAS[2] + P[13][5]*SH_TAS[1] - P[13][15]*SH_TAS[1] + P[13][6]*vd*SH_TAS[0]); - Kfusion[14] = SK_TAS*(P[14][4]*SH_TAS[2] - P[14][14]*SH_TAS[2] + P[14][5]*SH_TAS[1] - P[14][15]*SH_TAS[1] + P[14][6]*vd*SH_TAS[0]); - Kfusion[15] = SK_TAS*(P[15][4]*SH_TAS[2] - P[15][14]*SH_TAS[2] + P[15][5]*SH_TAS[1] - P[15][15]*SH_TAS[1] + P[15][6]*vd*SH_TAS[0]); - Kfusion[16] = SK_TAS*(P[16][4]*SH_TAS[2] - P[16][14]*SH_TAS[2] + P[16][5]*SH_TAS[1] - P[16][15]*SH_TAS[1] + P[16][6]*vd*SH_TAS[0]); - Kfusion[17] = SK_TAS*(P[17][4]*SH_TAS[2] - P[17][14]*SH_TAS[2] + P[17][5]*SH_TAS[1] - P[17][15]*SH_TAS[1] + P[17][6]*vd*SH_TAS[0]); - Kfusion[18] = SK_TAS*(P[18][4]*SH_TAS[2] - P[18][14]*SH_TAS[2] + P[18][5]*SH_TAS[1] - P[18][15]*SH_TAS[1] + P[18][6]*vd*SH_TAS[0]); - Kfusion[19] = SK_TAS*(P[19][4]*SH_TAS[2] - P[19][14]*SH_TAS[2] + P[19][5]*SH_TAS[1] - P[19][15]*SH_TAS[1] + P[19][6]*vd*SH_TAS[0]); - Kfusion[20] = SK_TAS*(P[20][4]*SH_TAS[2] - P[20][14]*SH_TAS[2] + P[20][5]*SH_TAS[1] - P[20][15]*SH_TAS[1] + P[20][6]*vd*SH_TAS[0]); - Kfusion[21] = SK_TAS*(P[21][4]*SH_TAS[2] - P[21][14]*SH_TAS[2] + P[21][5]*SH_TAS[1] - P[21][15]*SH_TAS[1] + P[21][6]*vd*SH_TAS[0]); - Kfusion[22] = SK_TAS*(P[22][4]*SH_TAS[2] - P[22][14]*SH_TAS[2] + P[22][5]*SH_TAS[1] - P[22][15]*SH_TAS[1] + P[22][6]*vd*SH_TAS[0]); + // Estimation of selected states is inhibited by setting their Kalman gains to zero + if (!inhibitWindStates) { + Kfusion[14] = SK_TAS*(P[14][4]*SH_TAS[2] - P[14][14]*SH_TAS[2] + P[14][5]*SH_TAS[1] - P[14][15]*SH_TAS[1] + P[14][6]*vd*SH_TAS[0]); + Kfusion[15] = SK_TAS*(P[15][4]*SH_TAS[2] - P[15][14]*SH_TAS[2] + P[15][5]*SH_TAS[1] - P[15][15]*SH_TAS[1] + P[15][6]*vd*SH_TAS[0]); + } else { + Kfusion[14] = 0; + Kfusion[15] = 0; + } + if (!inhibitMagStates) { + Kfusion[16] = SK_TAS*(P[16][4]*SH_TAS[2] - P[16][14]*SH_TAS[2] + P[16][5]*SH_TAS[1] - P[16][15]*SH_TAS[1] + P[16][6]*vd*SH_TAS[0]); + Kfusion[17] = SK_TAS*(P[17][4]*SH_TAS[2] - P[17][14]*SH_TAS[2] + P[17][5]*SH_TAS[1] - P[17][15]*SH_TAS[1] + P[17][6]*vd*SH_TAS[0]); + Kfusion[18] = SK_TAS*(P[18][4]*SH_TAS[2] - P[18][14]*SH_TAS[2] + P[18][5]*SH_TAS[1] - P[18][15]*SH_TAS[1] + P[18][6]*vd*SH_TAS[0]); + Kfusion[19] = SK_TAS*(P[19][4]*SH_TAS[2] - P[19][14]*SH_TAS[2] + P[19][5]*SH_TAS[1] - P[19][15]*SH_TAS[1] + P[19][6]*vd*SH_TAS[0]); + Kfusion[20] = SK_TAS*(P[20][4]*SH_TAS[2] - P[20][14]*SH_TAS[2] + P[20][5]*SH_TAS[1] - P[20][15]*SH_TAS[1] + P[20][6]*vd*SH_TAS[0]); + Kfusion[21] = SK_TAS*(P[21][4]*SH_TAS[2] - P[21][14]*SH_TAS[2] + P[21][5]*SH_TAS[1] - P[21][15]*SH_TAS[1] + P[21][6]*vd*SH_TAS[0]); + } else { + for (uint8_t i=16; i <= 21; i++) { + Kfusion[i] = 0; + } + } + if (!inhibitGndHgtState) { + Kfusion[22] = SK_TAS*(P[22][4]*SH_TAS[2] - P[22][14]*SH_TAS[2] + P[22][5]*SH_TAS[1] - P[22][15]*SH_TAS[1] + P[22][6]*vd*SH_TAS[0]); + } else { + Kfusion[22] = 0; + } varInnovVtas = 1.0f/SK_TAS; // Calculate the measurement innovation @@ -1662,9 +1781,9 @@ void AttPosEKF::FuseRangeFinder() float ptd = statesAtRngTime[22]; // Need to check that our range finder tilt angle is less than 30 degrees and we are using range finder data - SH_RNG[4] = sin(rngFinderPitch); + SH_RNG[4] = sinf(rngFinderPitch); cosRngTilt = - Tbn.z.x * SH_RNG[4] + Tbn.z.z * cosf(rngFinderPitch); - if (useRangeFinder && cosRngTilt > 0.87f) + if (useRangeFinder && fuseRngData && cosRngTilt > 0.87f) { // Calculate observation jacobian and Kalman gain ignoring all states other than the terrain offset // This prevents the range finder measurement modifying any of the other filter states and significantly reduces computations @@ -1685,10 +1804,12 @@ void AttPosEKF::FuseRangeFinder() SK_RNG[5] = SH_RNG[2]; Kfusion[22] = SK_RNG[0]*(P[22][9]*SH_RNG[3] - P[22][22]*SH_RNG[3] + P[22][0]*SH_RNG[1]*SK_RNG[2]*SK_RNG[5] - P[22][1]*SH_RNG[1]*SK_RNG[1]*SK_RNG[5] - P[22][2]*SH_RNG[1]*SK_RNG[4]*SK_RNG[5] + P[22][3]*SH_RNG[1]*SK_RNG[3]*SK_RNG[5]); + // Calculate the innovation variance for data logging + varInnovRng = 1.0f/SK_RNG[0]; + // Calculate the measurement innovation rngPred = (ptd - pd)/cosRngTilt; innovRng = rngPred - rngMea; - //printf("mea=%5.1f, pred=%5.1f, pd=%5.1f, ptd=%5.2f\n", rngMea, rngPred, pd, ptd); // Check the innovation for consistency and don't fuse if > 5Sigma if ((innovRng*innovRng*SK_RNG[0]) < 25) @@ -1704,6 +1825,289 @@ void AttPosEKF::FuseRangeFinder() } +void AttPosEKF::FuseOptFlow() +{ + static uint8_t obsIndex; + static float SH_LOS[13]; + static float SKK_LOS[15]; + static float SK_LOS[2]; + static float q0 = 0.0f; + static float q1 = 0.0f; + static float q2 = 0.0f; + static float q3 = 1.0f; + static float vn = 0.0f; + static float ve = 0.0f; + static float vd = 0.0f; + static float pd = 0.0f; + static float ptd = 0.0f; + static float R_LOS = 0.01f; + static float losPred[2]; + + // Transformation matrix from nav to body axes + Mat3f Tnb_local; + // Transformation matrix from body to sensor axes + // assume camera is aligned with Z body axis plus a misalignment + // defined by 3 small angles about X, Y and Z body axis + Mat3f Tbs; + Tbs.x.y = a3; + Tbs.y.x = -a3; + Tbs.x.z = -a2; + Tbs.z.x = a2; + Tbs.y.z = a1; + Tbs.z.y = -a1; + // Transformation matrix from navigation to sensor axes + Mat3f Tns; + float H_LOS[n_states]; + for (uint8_t i = 0; i < n_states; i++) { + H_LOS[i] = 0.0f; + } + Vector3f velNED_local; + Vector3f relVelSensor; + +// Perform sequential fusion of optical flow measurements only when in the air and tilt is less than 30 deg. + if (useOpticalFlow && (fuseOptFlowData || obsIndex == 1) && !onGround && Tbs.z.z > 0.866f && rngMea > 5.0f && rngMea < 39.0f) + { + // Sequential fusion of XY components to spread processing load across + // two prediction time steps. + + // Calculate observation jacobians and Kalman gains + if (fuseOptFlowData) + { + // Copy required states to local variable names + q0 = statesAtOptFlowTime[0]; + q1 = statesAtOptFlowTime[1]; + q2 = statesAtOptFlowTime[2]; + q3 = statesAtOptFlowTime[3]; + vn = statesAtOptFlowTime[4]; + ve = statesAtOptFlowTime[5]; + vd = statesAtOptFlowTime[6]; + pd = statesAtOptFlowTime[9]; + ptd = statesAtOptFlowTime[22]; + velNED_local.x = vn; + velNED_local.y = ve; + velNED_local.z = vd; + + // calculate rotation from NED to body axes + float q00 = sq(q0); + float q11 = sq(q1); + float q22 = sq(q2); + float q33 = sq(q3); + float q01 = q0 * q1; + float q02 = q0 * q2; + float q03 = q0 * q3; + float q12 = q1 * q2; + float q13 = q1 * q3; + float q23 = q2 * q3; + Tnb_local.x.x = q00 + q11 - q22 - q33; + Tnb_local.y.y = q00 - q11 + q22 - q33; + Tnb_local.z.z = q00 - q11 - q22 + q33; + Tnb_local.y.x = 2*(q12 - q03); + Tnb_local.z.x = 2*(q13 + q02); + Tnb_local.x.y = 2*(q12 + q03); + Tnb_local.z.y = 2*(q23 - q01); + Tnb_local.x.z = 2*(q13 - q02); + Tnb_local.y.z = 2*(q23 + q01); + + // calculate transformation from NED to sensor axes + Tns = Tbs*Tnb_local; + + // calculate range from ground plain to centre of sensor fov assuming flat earth + float range = ConstrainFloat(((ptd - pd)/Tns.z.z),0.5f,100.0f); + + // calculate relative velocity in sensor frame + relVelSensor = Tns*velNED_local; + + // divide velocity by range and include angular rate effects to get predicted angular LOS rates relative to X and Y axes + losPred[0] = relVelSensor.y/range; + losPred[1] = -relVelSensor.x/range; + + //printf("relVelSensor.x=%5.1f, relVelSensor.y=%5.1f\n", relVelSensor.x, relVelSensor.y); + //printf("Xpred=%5.2f, Xmea=%5.2f, Ypred=%5.2f, Ymea=%5.2f, delAng.x=%4.4f, delAng.y=%4.4f\n", losPred[0], losData[0], losPred[1], losData[1], delAng.x, delAng.y); + //printf("omegaX=%5.2f, omegaY=%5.2f, velY=%5.1f velX=%5.1f\n, range=%5.1f\n", delAngRel.x/dt, delAngRel.y/dt, relVelSensor.y, relVelSensor.x, range); + + // Calculate observation jacobians + SH_LOS[0] = a1*(2*q0*q1 + 2*q2*q3) + a2*(2*q0*q2 - 2*q1*q3) - sq(q0) + sq(q1) + sq(q2) - sq(q3); + SH_LOS[1] = vd*(a2*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q2 - 2*q1*q3 - a3*(2*q0*q1 + 2*q2*q3)) - ve*(a3*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + 2*q0*q3 + 2*q1*q2 + a2*(2*q0*q1 - 2*q2*q3)) + vn*(a2*(2*q0*q2 + 2*q1*q3) + a3*(2*q0*q3 - 2*q1*q2) - sq(q0) - sq(q1) + sq(q2) + sq(q3)); + SH_LOS[2] = ve*(a1*(2*q0*q1 - 2*q2*q3) + a3*(2*q0*q3 + 2*q1*q2) - sq(q0) + sq(q1) - sq(q2) + sq(q3)) - vd*(a1*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q1 + 2*q2*q3 + a3*(2*q0*q2 - 2*q1*q3)) + vn*(a3*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) + 2*q0*q3 - 2*q1*q2 - a1*(2*q0*q2 + 2*q1*q3)); + SH_LOS[3] = 1/(pd - ptd); + SH_LOS[4] = 2*q1 - 2*a2*q3 + 2*a3*q2; + SH_LOS[5] = 2*a2*q2 - 2*q0 + 2*a3*q3; + SH_LOS[6] = 2*q2 + 2*a2*q0 - 2*a3*q1; + SH_LOS[7] = 1/sq(pd - ptd); + SH_LOS[8] = 2*q2 + 2*a1*q3 - 2*a3*q1; + SH_LOS[9] = 2*q3 - 2*a1*q2 + 2*a3*q0; + SH_LOS[10] = 2*a1*q1 - 2*q0 + 2*a3*q3; + SH_LOS[11] = 2*q3 + 2*a2*q1 + 2*a3*q0; + SH_LOS[12] = 2*q1 + 2*a1*q0 + 2*a3*q2; + + for (uint8_t i = 0; i < n_states; i++) H_LOS[i] = 0; + H_LOS[0] = - SH_LOS[2]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) - SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[10] - vd*SH_LOS[12] + vn*SH_LOS[9]); + H_LOS[1] = - SH_LOS[2]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[10] + ve*SH_LOS[12] - vn*SH_LOS[8]); + H_LOS[2] = SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[9] + ve*SH_LOS[8] + vn*SH_LOS[12]) - SH_LOS[2]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3); + H_LOS[3] = SH_LOS[2]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[8] - ve*SH_LOS[9] + vn*SH_LOS[10]); + H_LOS[4] = -SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) + 2*q0*q3 - 2*q1*q2 - a1*(2*q0*q2 + 2*q1*q3)); + H_LOS[5] = -SH_LOS[0]*SH_LOS[3]*(a1*(2*q0*q1 - 2*q2*q3) + a3*(2*q0*q3 + 2*q1*q2) - sq(q0) + sq(q1) - sq(q2) + sq(q3)); + H_LOS[6] = SH_LOS[0]*SH_LOS[3]*(a1*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q1 + 2*q2*q3 + a3*(2*q0*q2 - 2*q1*q3)); + H_LOS[9] = SH_LOS[0]*SH_LOS[2]*SH_LOS[7]; + H_LOS[22] = -SH_LOS[0]*SH_LOS[2]*SH_LOS[7]; + + // Calculate Kalman gain + SKK_LOS[0] = a2*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q2 - 2*q1*q3 - a3*(2*q0*q1 + 2*q2*q3); + SKK_LOS[1] = a3*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + 2*q0*q3 + 2*q1*q2 + a2*(2*q0*q1 - 2*q2*q3); + SKK_LOS[2] = a2*(2*q0*q2 + 2*q1*q3) + a3*(2*q0*q3 - 2*q1*q2) - sq(q0) - sq(q1) + sq(q2) + sq(q3); + SKK_LOS[3] = a1*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q1 + 2*q2*q3 + a3*(2*q0*q2 - 2*q1*q3); + SKK_LOS[4] = a1*(2*q0*q1 - 2*q2*q3) + a3*(2*q0*q3 + 2*q1*q2) - sq(q0) + sq(q1) - sq(q2) + sq(q3); + SKK_LOS[5] = a3*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) + 2*q0*q3 - 2*q1*q2 - a1*(2*q0*q2 + 2*q1*q3); + SKK_LOS[6] = SH_LOS[2]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[8] - ve*SH_LOS[9] + vn*SH_LOS[10]); + SKK_LOS[7] = SH_LOS[2]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[10] + ve*SH_LOS[12] - vn*SH_LOS[8]); + SKK_LOS[8] = SH_LOS[2]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[10] - vd*SH_LOS[12] + vn*SH_LOS[9]); + SKK_LOS[9] = SH_LOS[2]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[9] + ve*SH_LOS[8] + vn*SH_LOS[12]); + SKK_LOS[10] = SH_LOS[1]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[6] - ve*SH_LOS[11] + vn*SH_LOS[5]); + SKK_LOS[11] = SH_LOS[1]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[11] + ve*SH_LOS[6] + vn*SH_LOS[4]); + SKK_LOS[12] = SH_LOS[1]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[5] + ve*SH_LOS[4] - vn*SH_LOS[6]); + SKK_LOS[13] = SH_LOS[1]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) - SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[5] - vd*SH_LOS[4] + vn*SH_LOS[11]); + SKK_LOS[14] = SH_LOS[0]; + + SK_LOS[0] = 1/(R_LOS + SKK_LOS[8]*(P[0][0]*SKK_LOS[8] + P[1][0]*SKK_LOS[7] + P[2][0]*SKK_LOS[9] - P[3][0]*SKK_LOS[6] - P[9][0]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][0]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][0]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][0]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][0]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]) + SKK_LOS[7]*(P[0][1]*SKK_LOS[8] + P[1][1]*SKK_LOS[7] + P[2][1]*SKK_LOS[9] - P[3][1]*SKK_LOS[6] - P[9][1]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][1]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][1]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][1]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][1]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]) + SKK_LOS[9]*(P[0][2]*SKK_LOS[8] + P[1][2]*SKK_LOS[7] + P[2][2]*SKK_LOS[9] - P[3][2]*SKK_LOS[6] - P[9][2]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][2]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][2]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][2]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][2]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]) - SKK_LOS[6]*(P[0][3]*SKK_LOS[8] + P[1][3]*SKK_LOS[7] + P[2][3]*SKK_LOS[9] - P[3][3]*SKK_LOS[6] - P[9][3]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][3]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][3]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][3]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][3]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]) - SH_LOS[2]*SH_LOS[7]*SKK_LOS[14]*(P[0][9]*SKK_LOS[8] + P[1][9]*SKK_LOS[7] + P[2][9]*SKK_LOS[9] - P[3][9]*SKK_LOS[6] - P[9][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][9]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][9]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][9]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]) + SH_LOS[2]*SH_LOS[7]*SKK_LOS[14]*(P[0][22]*SKK_LOS[8] + P[1][22]*SKK_LOS[7] + P[2][22]*SKK_LOS[9] - P[3][22]*SKK_LOS[6] - P[9][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][22]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][22]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][22]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]) + SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14]*(P[0][4]*SKK_LOS[8] + P[1][4]*SKK_LOS[7] + P[2][4]*SKK_LOS[9] - P[3][4]*SKK_LOS[6] - P[9][4]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][4]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][4]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][4]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]) + SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14]*(P[0][5]*SKK_LOS[8] + P[1][5]*SKK_LOS[7] + P[2][5]*SKK_LOS[9] - P[3][5]*SKK_LOS[6] - P[9][5]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][5]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][5]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][5]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]) - SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]*(P[0][6]*SKK_LOS[8] + P[1][6]*SKK_LOS[7] + P[2][6]*SKK_LOS[9] - P[3][6]*SKK_LOS[6] - P[9][6]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][6]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][6]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][6]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14])); + Kfusion[0] = -SK_LOS[0]*(P[0][0]*SKK_LOS[8] + P[0][1]*SKK_LOS[7] - P[0][3]*SKK_LOS[6] + P[0][2]*SKK_LOS[9] - P[0][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[0][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[0][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[0][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[0][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); + Kfusion[1] = -SK_LOS[0]*(P[1][0]*SKK_LOS[8] + P[1][1]*SKK_LOS[7] - P[1][3]*SKK_LOS[6] + P[1][2]*SKK_LOS[9] - P[1][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[1][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[1][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[1][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[1][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); + Kfusion[2] = -SK_LOS[0]*(P[2][0]*SKK_LOS[8] + P[2][1]*SKK_LOS[7] - P[2][3]*SKK_LOS[6] + P[2][2]*SKK_LOS[9] - P[2][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[2][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[2][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[2][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[2][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); + Kfusion[3] = -SK_LOS[0]*(P[3][0]*SKK_LOS[8] + P[3][1]*SKK_LOS[7] - P[3][3]*SKK_LOS[6] + P[3][2]*SKK_LOS[9] - P[3][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[3][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[3][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[3][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[3][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); + Kfusion[4] = -SK_LOS[0]*(P[4][0]*SKK_LOS[8] + P[4][1]*SKK_LOS[7] - P[4][3]*SKK_LOS[6] + P[4][2]*SKK_LOS[9] - P[4][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[4][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[4][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); + Kfusion[5] = -SK_LOS[0]*(P[5][0]*SKK_LOS[8] + P[5][1]*SKK_LOS[7] - P[5][3]*SKK_LOS[6] + P[5][2]*SKK_LOS[9] - P[5][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[5][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[5][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[5][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); + Kfusion[6] = -SK_LOS[0]*(P[6][0]*SKK_LOS[8] + P[6][1]*SKK_LOS[7] - P[6][3]*SKK_LOS[6] + P[6][2]*SKK_LOS[9] - P[6][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[6][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[6][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[6][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); + Kfusion[7] = -SK_LOS[0]*(P[7][0]*SKK_LOS[8] + P[7][1]*SKK_LOS[7] - P[7][3]*SKK_LOS[6] + P[7][2]*SKK_LOS[9] - P[7][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[7][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[7][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[7][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[7][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); + Kfusion[8] = -SK_LOS[0]*(P[8][0]*SKK_LOS[8] + P[8][1]*SKK_LOS[7] - P[8][3]*SKK_LOS[6] + P[8][2]*SKK_LOS[9] - P[8][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[8][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[8][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[8][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[8][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); + Kfusion[9] = -SK_LOS[0]*(P[9][0]*SKK_LOS[8] + P[9][1]*SKK_LOS[7] - P[9][3]*SKK_LOS[6] + P[9][2]*SKK_LOS[9] - P[9][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[9][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[9][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[9][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[9][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); + Kfusion[10] = -SK_LOS[0]*(P[10][0]*SKK_LOS[8] + P[10][1]*SKK_LOS[7] - P[10][3]*SKK_LOS[6] + P[10][2]*SKK_LOS[9] - P[10][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[10][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[10][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[10][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[10][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); + Kfusion[11] = -SK_LOS[0]*(P[11][0]*SKK_LOS[8] + P[11][1]*SKK_LOS[7] - P[11][3]*SKK_LOS[6] + P[11][2]*SKK_LOS[9] - P[11][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[11][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[11][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[11][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[11][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); + Kfusion[12] = -SK_LOS[0]*(P[12][0]*SKK_LOS[8] + P[12][1]*SKK_LOS[7] - P[12][3]*SKK_LOS[6] + P[12][2]*SKK_LOS[9] - P[12][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[12][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[12][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[12][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[12][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); + Kfusion[13] = 0.0f;//-SK_LOS[0]*(P[13][0]*SKK_LOS[8] + P[13][1]*SKK_LOS[7] - P[13][3]*SKK_LOS[6] + P[13][2]*SKK_LOS[9] - P[13][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[13][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[13][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[13][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[13][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); + Kfusion[14] = -SK_LOS[0]*(P[14][0]*SKK_LOS[8] + P[14][1]*SKK_LOS[7] - P[14][3]*SKK_LOS[6] + P[14][2]*SKK_LOS[9] - P[14][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[14][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[14][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[14][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[14][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); + Kfusion[15] = -SK_LOS[0]*(P[15][0]*SKK_LOS[8] + P[15][1]*SKK_LOS[7] - P[15][3]*SKK_LOS[6] + P[15][2]*SKK_LOS[9] - P[15][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[15][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[15][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[15][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[15][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); + Kfusion[16] = -SK_LOS[0]*(P[16][0]*SKK_LOS[8] + P[16][1]*SKK_LOS[7] - P[16][3]*SKK_LOS[6] + P[16][2]*SKK_LOS[9] - P[16][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[16][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[16][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[16][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[16][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); + Kfusion[17] = -SK_LOS[0]*(P[17][0]*SKK_LOS[8] + P[17][1]*SKK_LOS[7] - P[17][3]*SKK_LOS[6] + P[17][2]*SKK_LOS[9] - P[17][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[17][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[17][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[17][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[17][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); + Kfusion[18] = -SK_LOS[0]*(P[18][0]*SKK_LOS[8] + P[18][1]*SKK_LOS[7] - P[18][3]*SKK_LOS[6] + P[18][2]*SKK_LOS[9] - P[18][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[18][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[18][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[18][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[18][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); + Kfusion[19] = -SK_LOS[0]*(P[19][0]*SKK_LOS[8] + P[19][1]*SKK_LOS[7] - P[19][3]*SKK_LOS[6] + P[19][2]*SKK_LOS[9] - P[19][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[19][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[19][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[19][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[19][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); + Kfusion[20] = -SK_LOS[0]*(P[20][0]*SKK_LOS[8] + P[20][1]*SKK_LOS[7] - P[20][3]*SKK_LOS[6] + P[20][2]*SKK_LOS[9] - P[20][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[20][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[20][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[20][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[20][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); + Kfusion[21] = -SK_LOS[0]*(P[21][0]*SKK_LOS[8] + P[21][1]*SKK_LOS[7] - P[21][3]*SKK_LOS[6] + P[21][2]*SKK_LOS[9] - P[21][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[21][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[21][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[21][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[21][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); + Kfusion[22] = -SK_LOS[0]*(P[22][0]*SKK_LOS[8] + P[22][1]*SKK_LOS[7] - P[22][3]*SKK_LOS[6] + P[22][2]*SKK_LOS[9] - P[22][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[22][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[22][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); + varInnovOptFlow[0] = 1.0f/SK_LOS[0]; + innovOptFlow[0] = losPred[0] - losData[0]; + + // reset the observation index to 0 (we start by fusing the X + // measurement) + obsIndex = 0; + fuseOptFlowData = false; + } + else if (obsIndex == 1) // we are now fusing the Y measurement + { + // Calculate observation jacobians + for (uint8_t i = 0; i < n_states; i++) H_LOS[i] = 0; + H_LOS[0] = SH_LOS[1]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[6] - ve*SH_LOS[11] + vn*SH_LOS[5]); + H_LOS[1] = SH_LOS[1]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[11] + ve*SH_LOS[6] + vn*SH_LOS[4]); + H_LOS[2] = SH_LOS[1]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[5] + ve*SH_LOS[4] - vn*SH_LOS[6]); + H_LOS[3] = SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[5] - vd*SH_LOS[4] + vn*SH_LOS[11]) - SH_LOS[1]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1); + H_LOS[4] = SH_LOS[0]*SH_LOS[3]*(a2*(2*q0*q2 + 2*q1*q3) + a3*(2*q0*q3 - 2*q1*q2) - sq(q0) - sq(q1) + sq(q2) + sq(q3)); + H_LOS[5] = -SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + 2*q0*q3 + 2*q1*q2 + a2*(2*q0*q1 - 2*q2*q3)); + H_LOS[6] = SH_LOS[0]*SH_LOS[3]*(a2*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q2 - 2*q1*q3 - a3*(2*q0*q1 + 2*q2*q3)); + H_LOS[9] = -SH_LOS[0]*SH_LOS[1]*SH_LOS[7]; + H_LOS[22] = SH_LOS[0]*SH_LOS[1]*SH_LOS[7]; + + // Calculate Kalman gains + SK_LOS[1] = 1/(R_LOS + SKK_LOS[12]*(P[0][2]*SKK_LOS[10] + P[1][2]*SKK_LOS[11] + P[2][2]*SKK_LOS[12] - P[3][2]*SKK_LOS[13] - P[9][2]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][2]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][2]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][2]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][2]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]) - SKK_LOS[13]*(P[0][3]*SKK_LOS[10] + P[1][3]*SKK_LOS[11] + P[2][3]*SKK_LOS[12] - P[3][3]*SKK_LOS[13] - P[9][3]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][3]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][3]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][3]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][3]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]) + SKK_LOS[10]*(P[0][0]*SKK_LOS[10] + P[1][0]*SKK_LOS[11] + P[2][0]*(SH_LOS[1]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[3]*SKK_LOS[14]*(vd*SH_LOS[5] + ve*SH_LOS[4] - vn*SH_LOS[6])) - P[3][0]*(SH_LOS[1]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) - SH_LOS[3]*SKK_LOS[14]*(ve*SH_LOS[5] - vd*SH_LOS[4] + vn*SH_LOS[11])) - P[9][0]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][0]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][0]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][0]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][0]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]) + SKK_LOS[11]*(P[0][1]*SKK_LOS[10] + P[1][1]*SKK_LOS[11] + P[2][1]*(SH_LOS[1]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[3]*SKK_LOS[14]*(vd*SH_LOS[5] + ve*SH_LOS[4] - vn*SH_LOS[6])) - P[3][1]*(SH_LOS[1]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) - SH_LOS[3]*SKK_LOS[14]*(ve*SH_LOS[5] - vd*SH_LOS[4] + vn*SH_LOS[11])) - P[9][1]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][1]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][1]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][1]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][1]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]) - SH_LOS[1]*SH_LOS[7]*SKK_LOS[14]*(P[0][9]*SKK_LOS[10] + P[1][9]*SKK_LOS[11] + P[2][9]*SKK_LOS[12] - P[3][9]*SKK_LOS[13] - P[9][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][9]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][9]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][9]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]) + SH_LOS[1]*SH_LOS[7]*SKK_LOS[14]*(P[0][22]*SKK_LOS[10] + P[1][22]*SKK_LOS[11] + P[2][22]*SKK_LOS[12] - P[3][22]*SKK_LOS[13] - P[9][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][22]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][22]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][22]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]) + SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14]*(P[0][4]*SKK_LOS[10] + P[1][4]*SKK_LOS[11] + P[2][4]*SKK_LOS[12] - P[3][4]*SKK_LOS[13] - P[9][4]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][4]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][4]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][4]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]) - SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]*(P[0][5]*SKK_LOS[10] + P[1][5]*SKK_LOS[11] + P[2][5]*SKK_LOS[12] - P[3][5]*SKK_LOS[13] - P[9][5]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][5]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][5]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][5]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]) + SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]*(P[0][6]*SKK_LOS[10] + P[1][6]*SKK_LOS[11] + P[2][6]*SKK_LOS[12] - P[3][6]*SKK_LOS[13] - P[9][6]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][6]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][6]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][6]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14])); + Kfusion[0] = SK_LOS[1]*(P[0][0]*SKK_LOS[10] - P[0][3]*SKK_LOS[13] + P[0][1]*SKK_LOS[11] + P[0][2]*SKK_LOS[12] - P[0][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[0][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[0][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[0][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[0][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); + Kfusion[1] = SK_LOS[1]*(P[1][0]*SKK_LOS[10] - P[1][3]*SKK_LOS[13] + P[1][1]*SKK_LOS[11] + P[1][2]*SKK_LOS[12] - P[1][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[1][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[1][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[1][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[1][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); + Kfusion[2] = SK_LOS[1]*(P[2][0]*SKK_LOS[10] - P[2][3]*SKK_LOS[13] + P[2][1]*SKK_LOS[11] + P[2][2]*SKK_LOS[12] - P[2][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[2][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[2][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[2][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[2][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); + Kfusion[3] = SK_LOS[1]*(P[3][0]*SKK_LOS[10] - P[3][3]*SKK_LOS[13] + P[3][1]*SKK_LOS[11] + P[3][2]*SKK_LOS[12] - P[3][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[3][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[3][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[3][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[3][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); + Kfusion[4] = SK_LOS[1]*(P[4][0]*SKK_LOS[10] - P[4][3]*SKK_LOS[13] + P[4][1]*SKK_LOS[11] + P[4][2]*SKK_LOS[12] - P[4][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[4][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[4][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); + Kfusion[5] = SK_LOS[1]*(P[5][0]*SKK_LOS[10] - P[5][3]*SKK_LOS[13] + P[5][1]*SKK_LOS[11] + P[5][2]*SKK_LOS[12] - P[5][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[5][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[5][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[5][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); + Kfusion[6] = SK_LOS[1]*(P[6][0]*SKK_LOS[10] - P[6][3]*SKK_LOS[13] + P[6][1]*SKK_LOS[11] + P[6][2]*SKK_LOS[12] - P[6][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[6][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[6][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[6][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[6][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); + Kfusion[7] = SK_LOS[1]*(P[7][0]*SKK_LOS[10] - P[7][3]*SKK_LOS[13] + P[7][1]*SKK_LOS[11] + P[7][2]*SKK_LOS[12] - P[7][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[7][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[7][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[7][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[7][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); + Kfusion[8] = SK_LOS[1]*(P[8][0]*SKK_LOS[10] - P[8][3]*SKK_LOS[13] + P[8][1]*SKK_LOS[11] + P[8][2]*SKK_LOS[12] - P[8][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[8][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[8][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[8][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[8][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); + Kfusion[9] = SK_LOS[1]*(P[9][0]*SKK_LOS[10] - P[9][3]*SKK_LOS[13] + P[9][1]*SKK_LOS[11] + P[9][2]*SKK_LOS[12] - P[9][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[9][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[9][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[9][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[9][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); + Kfusion[10] = SK_LOS[1]*(P[10][0]*SKK_LOS[10] - P[10][3]*SKK_LOS[13] + P[10][1]*SKK_LOS[11] + P[10][2]*SKK_LOS[12] - P[10][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[10][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[10][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[10][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[10][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); + Kfusion[11] = SK_LOS[1]*(P[11][0]*SKK_LOS[10] - P[11][3]*SKK_LOS[13] + P[11][1]*SKK_LOS[11] + P[11][2]*SKK_LOS[12] - P[11][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[11][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[11][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[11][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[11][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); + Kfusion[12] = SK_LOS[1]*(P[12][0]*SKK_LOS[10] - P[12][3]*SKK_LOS[13] + P[12][1]*SKK_LOS[11] + P[12][2]*SKK_LOS[12] - P[12][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[12][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[12][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[12][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[12][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); + Kfusion[13] = 0.0f;//SK_LOS[1]*(P[13][0]*SKK_LOS[10] - P[13][3]*SKK_LOS[13] + P[13][1]*SKK_LOS[11] + P[13][2]*SKK_LOS[12] - P[13][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[13][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[13][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[13][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[13][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); + Kfusion[14] = SK_LOS[1]*(P[14][0]*SKK_LOS[10] - P[14][3]*SKK_LOS[13] + P[14][1]*SKK_LOS[11] + P[14][2]*SKK_LOS[12] - P[14][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[14][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[14][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[14][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[14][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); + Kfusion[15] = SK_LOS[1]*(P[15][0]*SKK_LOS[10] - P[15][3]*SKK_LOS[13] + P[15][1]*SKK_LOS[11] + P[15][2]*SKK_LOS[12] - P[15][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[15][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[15][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[15][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[15][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); + Kfusion[16] = SK_LOS[1]*(P[16][0]*SKK_LOS[10] - P[16][3]*SKK_LOS[13] + P[16][1]*SKK_LOS[11] + P[16][2]*SKK_LOS[12] - P[16][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[16][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[16][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[16][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[16][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); + Kfusion[17] = SK_LOS[1]*(P[17][0]*SKK_LOS[10] - P[17][3]*SKK_LOS[13] + P[17][1]*SKK_LOS[11] + P[17][2]*SKK_LOS[12] - P[17][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[17][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[17][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[17][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[17][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); + Kfusion[18] = SK_LOS[1]*(P[18][0]*SKK_LOS[10] - P[18][3]*SKK_LOS[13] + P[18][1]*SKK_LOS[11] + P[18][2]*SKK_LOS[12] - P[18][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[18][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[18][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[18][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[18][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); + Kfusion[19] = SK_LOS[1]*(P[19][0]*SKK_LOS[10] - P[19][3]*SKK_LOS[13] + P[19][1]*SKK_LOS[11] + P[19][2]*SKK_LOS[12] - P[19][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[19][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[19][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[19][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[19][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); + Kfusion[20] = SK_LOS[1]*(P[20][0]*SKK_LOS[10] - P[20][3]*SKK_LOS[13] + P[20][1]*SKK_LOS[11] + P[20][2]*SKK_LOS[12] - P[20][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[20][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[20][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[20][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[20][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); + Kfusion[21] = SK_LOS[1]*(P[21][0]*SKK_LOS[10] - P[21][3]*SKK_LOS[13] + P[21][1]*SKK_LOS[11] + P[21][2]*SKK_LOS[12] - P[21][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[21][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[21][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[21][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[21][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); + Kfusion[22] = SK_LOS[1]*(P[22][0]*SKK_LOS[10] - P[22][3]*SKK_LOS[13] + P[22][1]*SKK_LOS[11] + P[22][2]*SKK_LOS[12] - P[22][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[22][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[22][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); + varInnovOptFlow[1] = 1.0f/SK_LOS[1]; + innovOptFlow[1] = losPred[1] - losData[1]; + } + + // Check the innovation for consistency and don't fuse if > 3Sigma + if ((innovOptFlow[obsIndex]*innovOptFlow[obsIndex]/varInnovOptFlow[obsIndex]) < 9.0f) + { + // correct the state vector + for (uint8_t j = 0; j < n_states; j++) + { + states[j] = states[j] - Kfusion[j] * innovOptFlow[obsIndex]; + } + // normalise the quaternion states + float quatMag = sqrt(states[0]*states[0] + states[1]*states[1] + states[2]*states[2] + states[3]*states[3]); + if (quatMag > 1e-12f) + { + for (uint8_t j= 0; j<=3; j++) + { + float quatMagInv = 1.0f/quatMag; + states[j] = states[j] * quatMagInv; + } + } + // correct the covariance P = (I - K*H)*P + // take advantage of the empty columns in KH to reduce the + // number of operations + for (uint8_t i = 0; i < n_states; i++) + { + for (uint8_t j = 0; j <= 6; j++) + { + KH[i][j] = Kfusion[i] * H_LOS[j]; + } + for (uint8_t j = 7; j <= 8; j++) + { + KH[i][j] = 0.0f; + } + KH[i][9] = Kfusion[i] * H_LOS[9]; + for (uint8_t j = 10; j <= 21; j++) + { + KH[i][j] = 0.0f; + } + KH[i][22] = Kfusion[i] * H_LOS[22]; + } + for (uint8_t i = 0; i < n_states; i++) + { + for (uint8_t j = 0; j < n_states; j++) + { + KHP[i][j] = 0.0f; + for (uint8_t k = 0; k <= 6; k++) + { + KHP[i][j] = KHP[i][j] + KH[i][k] * P[k][j]; + } + KHP[i][j] = KHP[i][j] + KH[i][9] * P[9][j]; + KHP[i][j] = KHP[i][j] + KH[i][22] * P[2][j]; + } + } + } + for (uint8_t i = 0; i < n_states; i++) + { + for (uint8_t j = 0; j < n_states; j++) + { + P[i][j] = P[i][j] - KHP[i][j]; + } + } + } + obsIndex = obsIndex + 1; + ForceSymmetry(); + ConstrainVariances(); +} + void AttPosEKF::zeroCols(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last) { uint8_t row; @@ -1900,10 +2304,28 @@ void AttPosEKF::calcLLH(float posNED[3], double &lat, double &lon, float &hgt, d void AttPosEKF::OnGroundCheck() { - onGround = (((sq(velNED[0]) + sq(velNED[1]) + sq(velNED[2])) < 4.0f) && (VtasMeas < 6.0f)); + onGround = (((sq(velNED[0]) + sq(velNED[1]) + sq(velNED[2])) < 4.0f) && (VtasMeas < 8.0f)); if (staticMode) { staticMode = (!refSet || (GPSstatus < GPS_FIX_3D)); } + // don't update wind states if there is no airspeed measurement + if (onGround || !useAirspeed) { + inhibitWindStates = true; + } else { + inhibitWindStates =false; + } + // don't update magnetic field states if on ground or not using compass + if (onGround || !useCompass) { + inhibitMagStates = true; + } else { + inhibitMagStates = false; + } + // don't update terrain offset state if on ground + if (onGround) { + inhibitGndHgtState = true; + } else { + inhibitGndHgtState = false; + } } void AttPosEKF::calcEarthRateNED(Vector3f &omega, float latitude) @@ -1931,8 +2353,8 @@ void AttPosEKF::CovarianceInit() P[11][11] = P[10][10]; P[12][12] = P[10][10]; P[13][13] = sq(0.2f*dtIMU); - P[14][14] = sq(8.0f); - P[15][14] = P[14][14]; + P[14][14] = sq(0.0f); + P[15][15] = P[14][14]; P[16][16] = sq(0.02f); P[17][17] = P[16][16]; P[18][18] = P[16][16]; @@ -1956,7 +2378,7 @@ float AttPosEKF::ConstrainFloat(float val, float min, float max) } if (!isfinite(val)) { - ekf_debug("constrain: non-finite!"); + //ekf_debug("constrain: non-finite!"); } return ret; @@ -2376,7 +2798,7 @@ int AttPosEKF::CheckAndBound(struct ekf_status_report *last_error) ResetHeight(); ResetStoredStates(); - ret = 3; + ret = 0; } // Reset the filter if gyro offsets are excessive @@ -2482,12 +2904,18 @@ void AttPosEKF::InitializeDynamic(float (&initvelNED)[3], float declination) current_ekf_state.statesNaN = false; current_ekf_state.velHealth = true; - //current_ekf_state.posHealth = ?; - //current_ekf_state.hgtHealth = ?; + current_ekf_state.posHealth = true; + current_ekf_state.hgtHealth = true; current_ekf_state.velTimeout = false; - //current_ekf_state.posTimeout = ?; - //current_ekf_state.hgtTimeout = ?; + current_ekf_state.posTimeout = false; + current_ekf_state.hgtTimeout = false; + + fuseVelData = false; + fusePosData = false; + fuseHgtData = false; + fuseMagData = false; + fuseVtasData = false; // Fill variables with valid data velNED[0] = initvelNED[0]; @@ -2503,12 +2931,12 @@ void AttPosEKF::InitializeDynamic(float (&initvelNED)[3], float declination) // Calculate initial Tbn matrix and rotate Mag measurements into NED // to set initial NED magnetic field states - Mat3f DCM; - quat2Tbn(DCM, initQuat); + quat2Tbn(Tbn, initQuat); + Tnb = Tbn.transpose(); Vector3f initMagNED; - initMagNED.x = DCM.x.x*initMagXYZ.x + DCM.x.y*initMagXYZ.y + DCM.x.z*initMagXYZ.z; - initMagNED.y = DCM.y.x*initMagXYZ.x + DCM.y.y*initMagXYZ.y + DCM.y.z*initMagXYZ.z; - initMagNED.z = DCM.z.x*initMagXYZ.x + DCM.z.y*initMagXYZ.y + DCM.z.z*initMagXYZ.z; + initMagNED.x = Tbn.x.x*initMagXYZ.x + Tbn.x.y*initMagXYZ.y + Tbn.x.z*initMagXYZ.z; + initMagNED.y = Tbn.y.x*initMagXYZ.x + Tbn.y.y*initMagXYZ.y + Tbn.y.z*initMagXYZ.z; + initMagNED.z = Tbn.z.x*initMagXYZ.x + Tbn.z.y*initMagXYZ.y + Tbn.z.z*initMagXYZ.z; magstate.q0 = initQuat[0]; magstate.q1 = initQuat[1]; @@ -2521,7 +2949,7 @@ void AttPosEKF::InitializeDynamic(float (&initvelNED)[3], float declination) magstate.magYbias = magBias.y; magstate.magZbias = magBias.z; magstate.R_MAG = sq(magMeasurementSigma); - magstate.DCM = DCM; + magstate.DCM = Tbn; // write to state vector for (uint8_t j=0; j<=3; j++) states[j] = initQuat[j]; // quaternions @@ -2596,6 +3024,8 @@ void AttPosEKF::ZeroVariables() correctedDelAng.zero(); summedDelAng.zero(); summedDelVel.zero(); + dAngIMU.zero(); + dVelIMU.zero(); lastGyroOffset.zero(); for (unsigned i = 0; i < data_buffer_size; i++) { @@ -2623,6 +3053,10 @@ void AttPosEKF::GetFilterState(struct ekf_status_report *err) current_ekf_state.states[i] = states[i]; } current_ekf_state.n_states = n_states; + current_ekf_state.onGround = onGround; + current_ekf_state.staticMode = staticMode; + current_ekf_state.useCompass = useCompass; + current_ekf_state.useAirspeed = useAirspeed; memcpy(err, ¤t_ekf_state, sizeof(*err)); diff --git a/src/modules/ekf_att_pos_estimator/estimator_23states.h b/src/modules/ekf_att_pos_estimator/estimator_23states.h index 15ceb57c0..faa6735ca 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_23states.h +++ b/src/modules/ekf_att_pos_estimator/estimator_23states.h @@ -29,6 +29,10 @@ public: float covDelAngMax; // maximum delta angle between covariance predictions float rngFinderPitch; // pitch angle of laser range finder in radians. Zero is aligned with the Z body axis. Positive is RH rotation about Y body axis. + float a1; // optical flow sensor misalgnment angle about X axis (rad) + float a2; // optical flow sensor misalgnment angle about Y axis (rad) + float a3; // optical flow sensor misalgnment angle about Z axis (rad) + float yawVarScale; float windVelSigma; float dAngBiasSigma; @@ -55,6 +59,9 @@ public: covDelAngMax = 0.02f; // maximum delta angle between covariance predictions rngFinderPitch = 0.0f; // pitch angle of laser range finder in radians. Zero is aligned with the Z body axis. Positive is RH rotation about Y body axis. EAS2TAS = 1.0f; + a1 = 0.0f; // optical flow sensor misalgnment angle about X axis (rad) + a2 = 0.0f; // optical flow sensor misalgnment angle about Y axis (rad) + a3 = 0.0f; // optical flow sensor misalgnment angle about Z axis (rad) yawVarScale = 1.0f; windVelSigma = 0.1f; @@ -115,6 +122,7 @@ public: float statesAtMagMeasTime[n_states]; // filter satates at the effective measurement time float statesAtVtasMeasTime[n_states]; // filter states at the effective measurement time float statesAtRngTime[n_states]; // filter states at the effective measurement time + float statesAtOptFlowTime[n_states]; // States at the effective optical flow measurement time Vector3f correctedDelAng; // delta angles about the xyz body axes corrected for errors (rad) Vector3f correctedDelVel; // delta velocities along the XYZ body axes corrected for errors (m/s) @@ -147,9 +155,13 @@ public: float innovMag[3]; // innovation output float varInnovMag[3]; // innovation variance output Vector3f magData; // magnetometer flux radings in X,Y,Z body axes + float losData[2]; // optical flow LOS rate measurements (rad/sec) float innovVtas; // innovation output float innovRng; ///< Range finder innovation + float innovOptFlow[2]; // optical flow LOS innovations (rad/sec) + float varInnovOptFlow[2]; // optical flow innovations variances (rad/sec)^2 float varInnovVtas; // innovation variance output + float varInnovRng; // range finder innovation variance float VtasMeas; // true airspeed measurement (m/s) float magDeclination; ///< magnetic declination double latRef; // WGS-84 latitude of reference point (rad) @@ -160,8 +172,6 @@ public: unsigned covSkipCount; // Number of state prediction frames (IMU daya updates to skip before doing the covariance prediction // GPS input data variables - float gpsCourse; - float gpsVelD; double gpsLat; double gpsLon; float gpsHgt; @@ -178,12 +188,18 @@ public: bool fuseMagData; // boolean true when magnetometer data is to be fused bool fuseVtasData; // boolean true when airspeed data is to be fused bool fuseRngData; ///< true when range data is fused + bool fuseOptFlowData; // true when optical flow data is fused + + bool inhibitWindStates; // true when wind states and covariances are to remain constant + bool inhibitMagStates; // true when magnetic field states and covariances are to remain constant + bool inhibitGndHgtState; // true when the terrain ground height offset state and covariances are to remain constant bool onGround; ///< boolean true when the flight vehicle is on the ground (not flying) bool staticMode; ///< boolean true if no position feedback is fused bool useAirspeed; ///< boolean true if airspeed data is being used bool useCompass; ///< boolean true if magnetometer data is being used bool useRangeFinder; ///< true when rangefinder is being used + bool useOpticalFlow; // true when optical flow data is being used bool ekfDiverged; uint64_t lastReset; @@ -208,7 +224,7 @@ void FuseAirspeed(); void FuseRangeFinder(); -void FuseOpticalFlow(); +void FuseOptFlow(); void zeroRows(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last); diff --git a/src/modules/ekf_att_pos_estimator/estimator_utilities.cpp b/src/modules/ekf_att_pos_estimator/estimator_utilities.cpp index b4767a0d3..77cc1eeeb 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_utilities.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator_utilities.cpp @@ -8,6 +8,7 @@ #ifdef EKF_DEBUG #include <stdio.h> +#include <stdarg.h> static void ekf_debug_print(const char *fmt, va_list args) @@ -44,8 +45,11 @@ void Vector3f::zero(void) z = 0.0f; } -Mat3f::Mat3f() { - identity(); +Mat3f::Mat3f() : + x{1.0f, 0.0f, 0.0f}, + y{0.0f, 1.0f, 0.0f}, + z{0.0f, 0.0f, 1.0f} +{ } void Mat3f::identity() { @@ -101,6 +105,25 @@ Vector3f operator*( Mat3f matIn, Vector3f vecIn) return vecOut; } +// overload * operator to provide a matrix product +Mat3f operator*( Mat3f matIn1, Mat3f matIn2) +{ + Mat3f matOut; + matOut.x.x = matIn1.x.x*matIn2.x.x + matIn1.x.y*matIn2.y.x + matIn1.x.z*matIn2.z.x; + matOut.x.y = matIn1.x.x*matIn2.x.y + matIn1.x.y*matIn2.y.y + matIn1.x.z*matIn2.z.y; + matOut.x.z = matIn1.x.x*matIn2.x.z + matIn1.x.y*matIn2.y.z + matIn1.x.z*matIn2.z.z; + + matOut.y.x = matIn1.y.x*matIn2.x.x + matIn1.y.y*matIn2.y.x + matIn1.y.z*matIn2.z.x; + matOut.y.y = matIn1.y.x*matIn2.x.y + matIn1.y.y*matIn2.y.y + matIn1.y.z*matIn2.z.y; + matOut.y.z = matIn1.y.x*matIn2.x.z + matIn1.y.y*matIn2.y.z + matIn1.y.z*matIn2.z.z; + + matOut.z.x = matIn1.z.x*matIn2.x.x + matIn1.z.y*matIn2.y.x + matIn1.z.z*matIn2.z.x; + matOut.z.y = matIn1.z.x*matIn2.x.y + matIn1.z.y*matIn2.y.y + matIn1.z.z*matIn2.z.y; + matOut.z.z = matIn1.z.x*matIn2.x.z + matIn1.z.y*matIn2.y.z + matIn1.z.z*matIn2.z.z; + + return matOut; +} + // overload % operator to provide a vector cross product Vector3f operator%( Vector3f vecIn1, Vector3f vecIn2) { diff --git a/src/modules/ekf_att_pos_estimator/estimator_utilities.h b/src/modules/ekf_att_pos_estimator/estimator_utilities.h index d47568b62..a6b670c4d 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_utilities.h +++ b/src/modules/ekf_att_pos_estimator/estimator_utilities.h @@ -19,6 +19,12 @@ public: float y; float z; + Vector3f(float a=0.0f, float b=0.0f, float c=0.0f) : + x(a), + y(b), + z(c) + {} + float length(void) const; void zero(void); }; @@ -41,6 +47,7 @@ Vector3f operator*(float sclIn1, Vector3f vecIn1); Vector3f operator+( Vector3f vecIn1, Vector3f vecIn2); Vector3f operator-( Vector3f vecIn1, Vector3f vecIn2); Vector3f operator*( Mat3f matIn, Vector3f vecIn); +Mat3f operator*( Mat3f matIn1, Mat3f matIn2); Vector3f operator%( Vector3f vecIn1, Vector3f vecIn2); Vector3f operator*(Vector3f vecIn1, float sclIn1); @@ -61,6 +68,10 @@ struct ekf_status_report { bool posTimeout; bool hgtTimeout; bool imuTimeout; + bool onGround; + bool staticMode; + bool useCompass; + bool useAirspeed; uint32_t velFailTime; uint32_t posFailTime; uint32_t hgtFailTime; @@ -79,4 +90,4 @@ struct ekf_status_report { bool velOffsetExcessive; }; -void ekf_debug(const char *fmt, ...);
\ No newline at end of file +void ekf_debug(const char *fmt, ...); diff --git a/src/modules/ekf_att_pos_estimator/module.mk b/src/modules/ekf_att_pos_estimator/module.mk index dc5220bf0..36d854ddd 100644 --- a/src/modules/ekf_att_pos_estimator/module.mk +++ b/src/modules/ekf_att_pos_estimator/module.mk @@ -41,3 +41,5 @@ SRCS = ekf_att_pos_estimator_main.cpp \ ekf_att_pos_estimator_params.c \ estimator_23states.cpp \ estimator_utilities.cpp + +EXTRACXXFLAGS = -Weffc++ 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 3cd4ce928..0cea13cc4 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -1,7 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013 PX4 Development Team. All rights reserved. - * Author: Lorenz Meier + * 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 @@ -99,13 +98,21 @@ public: /** * Start the sensors task. * - * @return OK on success. + * @return OK on success. */ int start(); + /** + * Task status + * + * @return true if the mainloop is running + */ + bool task_running() { return _task_running; } + private: bool _task_should_exit; /**< if true, sensor task should exit */ + bool _task_running; /**< if true, task is running in its mainloop */ int _control_task; /**< task handle for sensor task */ int _att_sub; /**< vehicle attitude subscription */ @@ -276,6 +283,7 @@ private: * Main sensor collection task. */ void task_main(); + }; namespace att_control @@ -287,12 +295,13 @@ namespace att_control #endif static const int ERROR = -1; -FixedwingAttitudeControl *g_control; +FixedwingAttitudeControl *g_control = nullptr; } FixedwingAttitudeControl::FixedwingAttitudeControl() : _task_should_exit(false), + _task_running(false), _control_task(-1), /* subscriptions */ @@ -520,7 +529,7 @@ FixedwingAttitudeControl::vehicle_accel_poll() orb_check(_accel_sub, &accel_updated); if (accel_updated) { - orb_copy(ORB_ID(sensor_accel), _accel_sub, &_accel); + orb_copy(ORB_ID(sensor_accel0), _accel_sub, &_accel); } } @@ -568,7 +577,7 @@ FixedwingAttitudeControl::task_main() */ _att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); _att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); - _accel_sub = orb_subscribe(ORB_ID(sensor_accel)); + _accel_sub = orb_subscribe(ORB_ID(sensor_accel0)); _airspeed_sub = orb_subscribe(ORB_ID(airspeed)); _vcontrol_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); _params_sub = orb_subscribe(ORB_ID(parameter_update)); @@ -598,6 +607,8 @@ FixedwingAttitudeControl::task_main() fds[1].fd = _att_sub; fds[1].events = POLLIN; + _task_running = true; + while (!_task_should_exit) { static int loop_counter = 0; @@ -921,6 +932,7 @@ FixedwingAttitudeControl::task_main() warnx("exiting.\n"); _control_task = -1; + _task_running = false; _exit(0); } @@ -966,6 +978,14 @@ int fw_att_control_main(int argc, char *argv[]) err(1, "start failed"); } + /* avoid memory fragmentation by not exiting start handler until the task has fully started */ + while (att_control::g_control == nullptr || !att_control::g_control->task_running()) { + usleep(50000); + printf("."); + fflush(stdout); + } + printf("\n"); + exit(0); } diff --git a/src/modules/fw_att_control/module.mk b/src/modules/fw_att_control/module.mk index 1e600757e..89c6494c5 100644 --- a/src/modules/fw_att_control/module.mk +++ b/src/modules/fw_att_control/module.mk @@ -39,3 +39,5 @@ MODULE_COMMAND = fw_att_control SRCS = fw_att_control_main.cpp \ fw_att_control_params.c + +MODULE_STACKSIZE = 1200 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..350ce6dec 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 @@ -1,7 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013 PX4 Development Team. All rights reserved. - * Author: Lorenz Meier + * 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 @@ -43,8 +42,8 @@ * Proceedings of the AIAA Guidance, Navigation and Control * Conference, Aug 2004. AIAA-2004-4900. * - * Implementation for total energy control class: - * Thomas Gubler + * Original implementation for total energy control class: + * Paul Riseborough and Andrew Tridgell, 2013 (code in lib/external_lgpl) * * More details and acknowledgements in the referenced library headers. * @@ -88,10 +87,13 @@ #include <mavlink/mavlink_log.h> #include <launchdetection/LaunchDetector.h> #include <ecl/l1/ecl_l1_pos_controller.h> +#include <external_lgpl/tecs/tecs.h> #include <drivers/drv_range_finder.h> #include "landingslope.h" #include "mtecs/mTecs.h" +static int _control_task = -1; /**< task handle for sensor task */ + /** * L1 control app start / stop handling function @@ -116,20 +118,26 @@ public: /** * Start the sensors task. * - * @return OK on success. + * @return OK on success. + */ + static int start(); + + /** + * Task status + * + * @return true if the mainloop is running */ - int start(); + bool task_running() { return _task_running; } private: int _mavlink_fd; bool _task_should_exit; /**< if true, sensor task should exit */ - int _control_task; /**< task handle for sensor task */ + bool _task_running; /**< if true, task is running in its mainloop */ int _global_pos_sub; int _pos_sp_triplet_sub; int _att_sub; /**< vehicle attitude subscription */ - int _attitude_sub; /**< raw rc channels data subscription */ int _airspeed_sub; /**< airspeed subscription */ int _control_mode_sub; /**< vehicle status subscription */ int _params_sub; /**< notification of parameter updates */ @@ -138,6 +146,7 @@ private: int _range_finder_sub; /**< range finder subscription */ orb_advert_t _attitude_sp_pub; /**< attitude setpoint */ + orb_advert_t _tecs_status_pub; /**< TECS status publication */ orb_advert_t _nav_capabilities_pub; /**< navigation capabilities publication */ struct vehicle_attitude_s _att; /**< vehicle attitude */ @@ -153,18 +162,6 @@ private: perf_counter_t _loop_perf; /**< loop performance counter */ - /** manual control states */ - float _altctrl_hold_heading; /**< heading the system should hold in altctrl mode */ - double _loiter_hold_lat; - double _loiter_hold_lon; - float _loiter_hold_alt; - bool _loiter_hold; - - double _launch_lat; - double _launch_lon; - float _launch_alt; - bool _launch_valid; - /* land states */ /* not in non-abort mode for landing yet */ bool land_noreturn_horizontal; @@ -181,8 +178,8 @@ private: /* Landingslope object */ Landingslope landingslope; - float flare_curve_alt_rel_last; + /* heading hold */ float target_bearing; @@ -198,6 +195,7 @@ private: math::Matrix<3, 3> _R_nb; ///< current attitude ECL_L1_Pos_Controller _l1_control; + TECS _tecs; fwPosctrl::mTecs _mTecs; bool _was_pos_control_mode; @@ -206,9 +204,13 @@ private: float l1_damping; float time_const; + float time_const_throt; float min_sink_rate; float max_sink_rate; float max_climb_rate; + float climbout_diff; + float heightrate_p; + float speedrate_p; float throttle_damp; float integrator_gain; float vertical_accel_limit; @@ -228,12 +230,10 @@ private: float throttle_min; float throttle_max; float throttle_cruise; + float throttle_slew_max; float throttle_land_max; - float heightrate_p; - float speedrate_p; - float land_slope_angle; float land_H1_virt; float land_flare_alt_relative; @@ -249,9 +249,13 @@ private: param_t l1_damping; param_t time_const; + param_t time_const_throt; param_t min_sink_rate; param_t max_sink_rate; param_t max_climb_rate; + param_t climbout_diff; + param_t heightrate_p; + param_t speedrate_p; param_t throttle_damp; param_t integrator_gain; param_t vertical_accel_limit; @@ -271,12 +275,10 @@ private: param_t throttle_min; param_t throttle_max; param_t throttle_cruise; + param_t throttle_slew_max; param_t throttle_land_max; - param_t heightrate_p; - param_t speedrate_p; - param_t land_slope_angle; param_t land_H1_virt; param_t land_flare_alt_relative; @@ -391,14 +393,14 @@ namespace l1_control #endif static const int ERROR = -1; -FixedwingPositionControl *g_control; +FixedwingPositionControl *g_control = nullptr; } FixedwingPositionControl::FixedwingPositionControl() : _mavlink_fd(-1), _task_should_exit(false), - _control_task(-1), + _task_running(false), /* subscriptions */ _global_pos_sub(-1), @@ -408,17 +410,29 @@ FixedwingPositionControl::FixedwingPositionControl() : _control_mode_sub(-1), _params_sub(-1), _manual_control_sub(-1), + _sensor_combined_sub(-1), _range_finder_sub(-1), /* publications */ _attitude_sp_pub(-1), + _tecs_status_pub(-1), _nav_capabilities_pub(-1), +/* states */ + _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")), -/* states */ - _loiter_hold(false), land_noreturn_horizontal(false), land_noreturn_vertical(false), land_stayonground(false), @@ -427,24 +441,18 @@ FixedwingPositionControl::FixedwingPositionControl() : launch_detected(false), usePreTakeoffThrust(false), last_manual(false), + landingslope(), flare_curve_alt_rel_last(0.0f), + target_bearing(0.0f), launchDetector(), _airspeed_error(0.0f), _airspeed_valid(false), + _airspeed_last_valid(0), _groundspeed_undershoot(0.0f), _global_pos_valid(false), - _att(), - _att_sp(), - _nav_capabilities(), - _manual(), - _airspeed(), - _control_mode(), - _global_pos(), - _pos_sp_triplet(), - _sensor_combined(), + _l1_control(), _mTecs(), - _was_pos_control_mode(false), - _range_finder() + _was_pos_control_mode(false) { _nav_capabilities.turn_distance = 0.0f; @@ -460,6 +468,7 @@ FixedwingPositionControl::FixedwingPositionControl() : _parameter_handles.roll_limit = param_find("FW_R_LIM"); _parameter_handles.throttle_min = param_find("FW_THR_MIN"); _parameter_handles.throttle_max = param_find("FW_THR_MAX"); + _parameter_handles.throttle_slew_max = param_find("FW_THR_SLEW_MAX"); _parameter_handles.throttle_cruise = param_find("FW_THR_CRUISE"); _parameter_handles.throttle_land_max = param_find("FW_THR_LND_MAX"); @@ -471,9 +480,11 @@ FixedwingPositionControl::FixedwingPositionControl() : _parameter_handles.range_finder_rel_alt = param_find("FW_LND_RFRALT"); _parameter_handles.time_const = param_find("FW_T_TIME_CONST"); + _parameter_handles.time_const_throt = param_find("FW_T_THRO_CONST"); _parameter_handles.min_sink_rate = param_find("FW_T_SINK_MIN"); _parameter_handles.max_sink_rate = param_find("FW_T_SINK_MAX"); _parameter_handles.max_climb_rate = param_find("FW_T_CLMB_MAX"); + _parameter_handles.climbout_diff = param_find("FW_CLMBOUT_DIFF"); _parameter_handles.throttle_damp = param_find("FW_T_THR_DAMP"); _parameter_handles.integrator_gain = param_find("FW_T_INTEG_GAIN"); _parameter_handles.vertical_accel_limit = param_find("FW_T_VERT_ACC"); @@ -532,10 +543,12 @@ FixedwingPositionControl::parameters_update() param_get(_parameter_handles.throttle_min, &(_parameters.throttle_min)); param_get(_parameter_handles.throttle_max, &(_parameters.throttle_max)); param_get(_parameter_handles.throttle_cruise, &(_parameters.throttle_cruise)); + param_get(_parameter_handles.throttle_slew_max, &(_parameters.throttle_slew_max)); param_get(_parameter_handles.throttle_land_max, &(_parameters.throttle_land_max)); param_get(_parameter_handles.time_const, &(_parameters.time_const)); + param_get(_parameter_handles.time_const_throt, &(_parameters.time_const_throt)); param_get(_parameter_handles.min_sink_rate, &(_parameters.min_sink_rate)); param_get(_parameter_handles.max_sink_rate, &(_parameters.max_sink_rate)); param_get(_parameter_handles.throttle_damp, &(_parameters.throttle_damp)); @@ -547,6 +560,7 @@ FixedwingPositionControl::parameters_update() param_get(_parameter_handles.speed_weight, &(_parameters.speed_weight)); param_get(_parameter_handles.pitch_damping, &(_parameters.pitch_damping)); param_get(_parameter_handles.max_climb_rate, &(_parameters.max_climb_rate)); + param_get(_parameter_handles.climbout_diff, &(_parameters.climbout_diff)); param_get(_parameter_handles.heightrate_p, &(_parameters.heightrate_p)); param_get(_parameter_handles.speedrate_p, &(_parameters.speedrate_p)); @@ -563,6 +577,25 @@ FixedwingPositionControl::parameters_update() _l1_control.set_l1_period(_parameters.l1_period); _l1_control.set_l1_roll_limit(math::radians(_parameters.roll_limit)); + _tecs.set_time_const(_parameters.time_const); + _tecs.set_time_const_throt(_parameters.time_const_throt); + _tecs.set_min_sink_rate(_parameters.min_sink_rate); + _tecs.set_max_sink_rate(_parameters.max_sink_rate); + _tecs.set_throttle_damp(_parameters.throttle_damp); + _tecs.set_throttle_slewrate(_parameters.throttle_slew_max); + _tecs.set_integrator_gain(_parameters.integrator_gain); + _tecs.set_vertical_accel_limit(_parameters.vertical_accel_limit); + _tecs.set_height_comp_filter_omega(_parameters.height_comp_filter_omega); + _tecs.set_speed_comp_filter_omega(_parameters.speed_comp_filter_omega); + _tecs.set_roll_throttle_compensation(_parameters.roll_throttle_compensation); + _tecs.set_speed_weight(_parameters.speed_weight); + _tecs.set_pitch_damping(_parameters.pitch_damping); + _tecs.set_indicated_airspeed_min(_parameters.airspeed_min); + _tecs.set_indicated_airspeed_max(_parameters.airspeed_max); + _tecs.set_max_climb_rate(_parameters.max_climb_rate); + _tecs.set_heightrate_p(_parameters.heightrate_p); + _tecs.set_speedrate_p(_parameters.speedrate_p); + /* sanity check parameters */ if (_parameters.airspeed_max < _parameters.airspeed_min || _parameters.airspeed_max < 5.0f || @@ -600,17 +633,7 @@ FixedwingPositionControl::vehicle_control_mode_poll() orb_check(_control_mode_sub, &vstatus_updated); if (vstatus_updated) { - - bool was_armed = _control_mode.flag_armed; - orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode); - - if (!was_armed && _control_mode.flag_armed) { - _launch_lat = _global_pos.lat; - _launch_lon = _global_pos.lon; - _launch_alt = _global_pos.alt; - _launch_valid = true; - } } } @@ -634,6 +657,9 @@ FixedwingPositionControl::vehicle_airspeed_poll() } } + /* update TECS state */ + _tecs.enable_airspeed(_airspeed_valid); + return airspeed_updated; } @@ -694,7 +720,17 @@ FixedwingPositionControl::vehicle_setpoint_poll() void FixedwingPositionControl::task_main_trampoline(int argc, char *argv[]) { + l1_control::g_control = new FixedwingPositionControl(); + + if (l1_control::g_control == nullptr) { + warnx("OUT OF MEM"); + return; + } + + /* only returns on exit */ l1_control::g_control->task_main(); + delete l1_control::g_control; + l1_control::g_control = nullptr; } float @@ -806,13 +842,15 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi float eas2tas = 1.0f; // XXX calculate actual number based on current measurements - // XXX re-visit - float baro_altitude = _global_pos.alt; - /* filter speed and altitude for controller */ math::Vector<3> accel_body(_sensor_combined.accelerometer_m_s2); math::Vector<3> accel_earth = _R_nb * accel_body; + if (!_mTecs.getEnabled()) { + _tecs.update_50hz(_global_pos.alt /* XXX might switch to alt err here */, _airspeed.indicated_airspeed_m_s, _R_nb, accel_body, accel_earth); + } + + /* define altitude error */ float altitude_error = _pos_sp_triplet.current.alt - _global_pos.alt; /* no throttle limit as default */ @@ -837,6 +875,9 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* get circle mode */ bool was_circle_mode = _l1_control.circle_mode(); + /* restore speed weight, in case changed intermittently (e.g. in landing handling) */ + _tecs.set_speed_weight(_parameters.speed_weight); + /* current waypoint (the one currently heading for) */ math::Vector<2> next_wp((float)pos_sp_triplet.current.lat, (float)pos_sp_triplet.current.lon); @@ -945,7 +986,6 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi float airspeed_approach = 1.3f * _parameters.airspeed_min; /* Calculate distance (to landing waypoint) and altitude of last ordinary waypoint L */ - float L_wp_distance = get_distance_to_next_waypoint(prev_wp(0), prev_wp(1), curr_wp(0), curr_wp(1)); float L_altitude_rel = _pos_sp_triplet.previous.valid ? _pos_sp_triplet.previous.alt - _pos_sp_triplet.current.alt : 0.0f; float bearing_airplane_currwp = get_bearing_to_next_waypoint(current_position(0), current_position(1), curr_wp(0), curr_wp(1)); @@ -1071,7 +1111,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi usePreTakeoffThrust = false; /* apply minimum pitch and limit roll if target altitude is not within 10 meters */ - if (altitude_error > 15.0f) { + if (_parameters.climbout_diff > 0.001f && altitude_error > _parameters.climbout_diff) { /* enforce a minimum of 10 degrees pitch up on takeoff, or take parameter */ tecs_update_pitch_throttle(_pos_sp_triplet.current.alt, @@ -1111,15 +1151,6 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } } - // warnx("nav bearing: %8.4f bearing err: %8.4f target bearing: %8.4f", (double)_l1_control.nav_bearing(), - // (double)_l1_control.bearing_error(), (double)_l1_control.target_bearing()); - // warnx("prev wp: %8.4f/%8.4f, next wp: %8.4f/%8.4f prev:%s", (double)prev_wp(0), (double)prev_wp(1), - // (double)next_wp(0), (double)next_wp(1), (pos_sp_triplet.previous_valid) ? "valid" : "invalid"); - - // XXX at this point we always want no loiter hold if a - // mission is active - _loiter_hold = false; - /* reset landing state */ if (pos_sp_triplet.current.type != SETPOINT_TYPE_LAND) { reset_landing_state(); @@ -1135,89 +1166,6 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi _att_sp.roll_reset_integral = true; } - } else if (0/* posctrl mode enabled */) { - - _was_pos_control_mode = false; - - /** POSCTRL FLIGHT **/ - - if (0/* switched from another mode to posctrl */) { - _altctrl_hold_heading = _att.yaw; - } - - if (0/* posctrl on and manual control yaw non-zero */) { - _altctrl_hold_heading = _att.yaw + _manual.r; - } - - //XXX not used - - /* climb out control */ -// bool climb_out = false; -// -// /* user wants to climb out */ -// if (_manual.pitch > 0.3f && _manual.throttle > 0.8f) { -// climb_out = true; -// } - - /* if in altctrl mode, set airspeed based on manual control */ - - // XXX check if ground speed undershoot should be applied here - float altctrl_airspeed = _parameters.airspeed_min + - (_parameters.airspeed_max - _parameters.airspeed_min) * - _manual.z; - - _l1_control.navigate_heading(_altctrl_hold_heading, _att.yaw, ground_speed_2d); - _att_sp.roll_body = _l1_control.nav_roll(); - _att_sp.yaw_body = _l1_control.nav_bearing(); - - tecs_update_pitch_throttle(_global_pos.alt + _manual.x * 2.0f, altctrl_airspeed, eas2tas, - math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max), - _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, - false, math::radians(_parameters.pitch_limit_min), _global_pos.alt, ground_speed); - - } else if (0/* altctrl mode enabled */) { - - _was_pos_control_mode = false; - - /** ALTCTRL FLIGHT **/ - - if (0/* switched from another mode to altctrl */) { - _altctrl_hold_heading = _att.yaw; - } - - if (0/* altctrl on and manual control yaw non-zero */) { - _altctrl_hold_heading = _att.yaw + _manual.r; - } - - /* if in altctrl mode, set airspeed based on manual control */ - - // XXX check if ground speed undershoot should be applied here - float altctrl_airspeed = _parameters.airspeed_min + - (_parameters.airspeed_max - _parameters.airspeed_min) * - _manual.z; - - /* user switched off throttle */ - if (_manual.z < 0.1f) { - throttle_max = 0.0f; - } - - /* climb out control */ - bool climb_out = false; - - /* user wants to climb out */ - if (_manual.x > 0.3f && _manual.z > 0.8f) { - climb_out = true; - } - - _l1_control.navigate_heading(_altctrl_hold_heading, _att.yaw, ground_speed_2d); - _att_sp.roll_body = _manual.y; - _att_sp.yaw_body = _manual.r; - tecs_update_pitch_throttle(_global_pos.alt + _manual.x * 2.0f, altctrl_airspeed, eas2tas, - math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max), - _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, - climb_out, math::radians(_parameters.pitch_limit_min), - _global_pos.alt, ground_speed); - } else { _was_pos_control_mode = false; @@ -1238,9 +1186,9 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi _att_sp.thrust = launchDetector.getThrottlePreTakeoff(); } else { - _att_sp.thrust = math::min(_mTecs.getThrottleSetpoint(), throttle_max); + _att_sp.thrust = math::min(_mTecs.getEnabled() ? _mTecs.getThrottleSetpoint() : _tecs.get_throttle_demand(), throttle_max); } - _att_sp.pitch_body = _mTecs.getPitchSetpoint(); + _att_sp.pitch_body = _mTecs.getEnabled() ? _mTecs.getPitchSetpoint() : _tecs.get_pitch_demand(); if (_control_mode.flag_control_position_enabled) { last_manual = false; @@ -1256,10 +1204,6 @@ void FixedwingPositionControl::task_main() { - /* inform about start */ - warnx("Initializing.."); - fflush(stdout); - /* * do subscriptions */ @@ -1294,6 +1238,8 @@ FixedwingPositionControl::task_main() fds[1].fd = _global_pos_sub; fds[1].events = POLLIN; + _task_running = true; + while (!_task_should_exit) { /* wait for up to 500ms for data */ @@ -1333,14 +1279,6 @@ FixedwingPositionControl::task_main() _mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); } - static uint64_t last_run = 0; - float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f; - last_run = hrt_absolute_time(); - - /* guard against too large deltaT's */ - if (deltaT > 1.0f) - deltaT = 0.01f; - /* load local copies */ orb_copy(ORB_ID(vehicle_global_position), _global_pos_sub, &_global_pos); @@ -1394,6 +1332,8 @@ FixedwingPositionControl::task_main() perf_end(_loop_perf); } + _task_running = false; + warnx("exiting.\n"); _control_task = -1; @@ -1424,20 +1364,74 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_ const math::Vector<3> &ground_speed, tecs_mode mode) { - /* Using mtecs library: prepare arguments for mtecs call */ - float flightPathAngle = 0.0f; - float ground_speed_length = ground_speed.length(); - if (ground_speed_length > FLT_EPSILON) { - flightPathAngle = -asinf(ground_speed(2)/ground_speed_length); - } - fwPosctrl::LimitOverride limitOverride; - if (climbout_mode) { - limitOverride.enablePitchMinOverride(M_RAD_TO_DEG_F * climbout_pitch_min_rad); + if (_mTecs.getEnabled()) { + /* Using mtecs library: prepare arguments for mtecs call */ + float flightPathAngle = 0.0f; + float ground_speed_length = ground_speed.length(); + if (ground_speed_length > FLT_EPSILON) { + flightPathAngle = -asinf(ground_speed(2)/ground_speed_length); + } + fwPosctrl::LimitOverride limitOverride; + if (climbout_mode) { + limitOverride.enablePitchMinOverride(M_RAD_TO_DEG_F * climbout_pitch_min_rad); + } else { + limitOverride.disablePitchMinOverride(); + } + _mTecs.updateAltitudeSpeed(flightPathAngle, altitude, alt_sp, _airspeed.true_airspeed_m_s, v_sp, mode, + limitOverride); } else { - limitOverride.disablePitchMinOverride(); + /* Using tecs library */ + _tecs.update_pitch_throttle(_R_nb, _att.pitch, altitude, alt_sp, v_sp, + _airspeed.indicated_airspeed_m_s, eas2tas, + climbout_mode, climbout_pitch_min_rad, + throttle_min, throttle_max, throttle_cruise, + pitch_min_rad, pitch_max_rad); + + struct TECS::tecs_state s; + _tecs.get_tecs_state(s); + + struct tecs_status_s t; + + t.timestamp = s.timestamp; + + switch (s.mode) { + case TECS::ECL_TECS_MODE_NORMAL: + t.mode = TECS_MODE_NORMAL; + break; + case TECS::ECL_TECS_MODE_UNDERSPEED: + t.mode = TECS_MODE_UNDERSPEED; + break; + case TECS::ECL_TECS_MODE_BAD_DESCENT: + t.mode = TECS_MODE_BAD_DESCENT; + break; + case TECS::ECL_TECS_MODE_CLIMBOUT: + t.mode = TECS_MODE_CLIMBOUT; + break; + } + + t.altitudeSp = s.hgt_dem; + t.altitude_filtered = s.hgt; + t.airspeedSp = s.spd_dem; + t.airspeed_filtered = s.spd; + + t.flightPathAngleSp = s.dhgt_dem; + t.flightPathAngle = s.dhgt; + t.flightPathAngleFiltered = s.dhgt; + + t.airspeedDerivativeSp = s.dspd_dem; + t.airspeedDerivative = s.dspd; + + t.totalEnergyRateSp = s.thr; + t.totalEnergyRate = s.ithr; + t.energyDistributionRateSp = s.ptch; + t.energyDistributionRate = s.iptch; + + if (_tecs_status_pub > 0) { + orb_publish(ORB_ID(tecs_status), _tecs_status_pub, &t); + } else { + _tecs_status_pub = orb_advertise(ORB_ID(tecs_status), &t); + } } - _mTecs.updateAltitudeSpeed(flightPathAngle, altitude, alt_sp, _airspeed.true_airspeed_m_s, v_sp, mode, - limitOverride); } int @@ -1449,7 +1443,7 @@ FixedwingPositionControl::start() _control_task = task_spawn_cmd("fw_pos_control_l1", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, - 4048, + 2000, (main_t)&FixedwingPositionControl::task_main_trampoline, nullptr); @@ -1471,17 +1465,18 @@ int fw_pos_control_l1_main(int argc, char *argv[]) if (l1_control::g_control != nullptr) errx(1, "already running"); - l1_control::g_control = new FixedwingPositionControl; - - if (l1_control::g_control == nullptr) - errx(1, "alloc failed"); - - if (OK != l1_control::g_control->start()) { - delete l1_control::g_control; - l1_control::g_control = nullptr; + if (OK != FixedwingPositionControl::start()) { err(1, "start failed"); } + /* avoid memory fragmentation by not exiting start handler until the task has fully started */ + while (l1_control::g_control == nullptr || !l1_control::g_control->task_running()) { + usleep(50000); + printf("."); + fflush(stdout); + } + printf("\n"); + exit(0); } diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c index 52128e1b7..41c374407 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c @@ -83,6 +83,17 @@ PARAM_DEFINE_FLOAT(FW_L1_DAMPING, 0.75f); PARAM_DEFINE_FLOAT(FW_THR_CRUISE, 0.7f); /** + * Throttle max slew rate + * + * Maximum slew rate for the commanded throttle + * + * @min 0.0 + * @max 1.0 + * @group L1 Control + */ +PARAM_DEFINE_FLOAT(FW_THR_SLEW_MAX, 0.0f); + +/** * Negative pitch limit * * The minimum negative pitch the controller will output. @@ -155,6 +166,18 @@ PARAM_DEFINE_FLOAT(FW_THR_MIN, 0.0f); PARAM_DEFINE_FLOAT(FW_THR_LND_MAX, 1.0f); /** + * Climbout Altitude difference + * + * If the altitude error exceeds this parameter, the system will climb out + * with maximum throttle and minimum airspeed until it is closer than this + * distance to the desired altitude. Mostly used for takeoff waypoints / modes. + * Set to zero to disable climbout mode (not recommended). + * + * @group L1 Control + */ +PARAM_DEFINE_FLOAT(FW_CLMBOUT_DIFF, 25.0f); + +/** * Maximum climb rate * * This is the best climb rate that the aircraft can achieve with @@ -181,7 +204,7 @@ PARAM_DEFINE_FLOAT(FW_T_CLMB_MAX, 5.0f); * set to THR_MIN and flown at the same airspeed as used * to measure FW_T_CLMB_MAX. * - * @group L1 Control + * @group Fixed Wing TECS */ PARAM_DEFINE_FLOAT(FW_T_SINK_MIN, 2.0f); @@ -194,7 +217,7 @@ PARAM_DEFINE_FLOAT(FW_T_SINK_MIN, 2.0f); * exceeding the lower pitch angle limit and without over-speeding * the aircraft. * - * @group L1 Control + * @group Fixed Wing TECS */ PARAM_DEFINE_FLOAT(FW_T_SINK_MAX, 5.0f); @@ -205,17 +228,28 @@ PARAM_DEFINE_FLOAT(FW_T_SINK_MAX, 5.0f); * Smaller values make it faster to respond, larger values make it slower * to respond. * - * @group L1 Control + * @group Fixed Wing TECS */ PARAM_DEFINE_FLOAT(FW_T_TIME_CONST, 5.0f); /** + * TECS Throttle time constant + * + * This is the time constant of the TECS throttle control algorithm (in seconds). + * Smaller values make it faster to respond, larger values make it slower + * to respond. + * + * @group Fixed Wing TECS + */ +PARAM_DEFINE_FLOAT(FW_T_THRO_CONST, 8.0f); + +/** * Throttle damping factor * * This is the damping gain for the throttle demand loop. * Increase to add damping to correct for oscillations in speed and height. * - * @group L1 Control + * @group Fixed Wing TECS */ PARAM_DEFINE_FLOAT(FW_T_THR_DAMP, 0.5f); @@ -227,7 +261,7 @@ PARAM_DEFINE_FLOAT(FW_T_THR_DAMP, 0.5f); * and height offsets are trimmed out, but reduces damping and * increases overshoot. * - * @group L1 Control + * @group Fixed Wing TECS */ PARAM_DEFINE_FLOAT(FW_T_INTEG_GAIN, 0.1f); @@ -240,7 +274,7 @@ PARAM_DEFINE_FLOAT(FW_T_INTEG_GAIN, 0.1f); * allows for reasonably aggressive pitch changes if required to recover * from under-speed conditions. * - * @group L1 Control + * @group Fixed Wing TECS */ PARAM_DEFINE_FLOAT(FW_T_VERT_ACC, 7.0f); @@ -253,7 +287,7 @@ PARAM_DEFINE_FLOAT(FW_T_VERT_ACC, 7.0f); * the solution more towards use of the barometer, whilst reducing it weights * the solution more towards use of the accelerometer data. * - * @group L1 Control + * @group Fixed Wing TECS */ PARAM_DEFINE_FLOAT(FW_T_HGT_OMEGA, 3.0f); @@ -266,7 +300,7 @@ PARAM_DEFINE_FLOAT(FW_T_HGT_OMEGA, 3.0f); * more towards use of the arispeed sensor, whilst reducing it weights the * solution more towards use of the accelerometer data. * - * @group L1 Control + * @group Fixed Wing TECS */ PARAM_DEFINE_FLOAT(FW_T_SPD_OMEGA, 2.0f); @@ -282,7 +316,7 @@ PARAM_DEFINE_FLOAT(FW_T_SPD_OMEGA, 2.0f); * aircraft (eg powered sailplanes) can use a lower value, whereas * inefficient low aspect-ratio models (eg delta wings) can use a higher value. * - * @group L1 Control + * @group Fixed Wing TECS */ PARAM_DEFINE_FLOAT(FW_T_RLL2THR, 10.0f); @@ -300,7 +334,7 @@ PARAM_DEFINE_FLOAT(FW_T_RLL2THR, 10.0f); * Note to Glider Pilots - set this parameter to 2.0 (The glider will * adjust its pitch angle to maintain airspeed, ignoring changes in height). * - * @group L1 Control + * @group Fixed Wing TECS */ PARAM_DEFINE_FLOAT(FW_T_SPDWEIGHT, 1.0f); @@ -312,21 +346,21 @@ PARAM_DEFINE_FLOAT(FW_T_SPDWEIGHT, 1.0f); * will work well provided the pitch to servo controller has been tuned * properly. * - * @group L1 Control + * @group Fixed Wing TECS */ PARAM_DEFINE_FLOAT(FW_T_PTCH_DAMP, 0.0f); /** * Height rate P factor * - * @group L1 Control + * @group Fixed Wing TECS */ PARAM_DEFINE_FLOAT(FW_T_HRATE_P, 0.05f); /** * Speed rate P factor * - * @group L1 Control + * @group Fixed Wing TECS */ PARAM_DEFINE_FLOAT(FW_T_SRATE_P, 0.05f); diff --git a/src/modules/fw_pos_control_l1/module.mk b/src/modules/fw_pos_control_l1/module.mk index af155fe08..15b575b50 100644 --- a/src/modules/fw_pos_control_l1/module.mk +++ b/src/modules/fw_pos_control_l1/module.mk @@ -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 @@ -43,3 +43,5 @@ SRCS = fw_pos_control_l1_main.cpp \ mtecs/mTecs.cpp \ mtecs/limitoverride.cpp \ mtecs/mTecs_params.c + +MODULE_STACKSIZE = 1200 diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp index fc0a2551c..bffeefc1f 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp @@ -59,6 +59,7 @@ mTecs::mTecs() : _controlAltitude(this, "FPA", true), _controlAirSpeed(this, "ACC"), _flightPathAngleLowpass(this, "FPA_LP"), + _altitudeLowpass(this, "ALT_LP"), _airspeedLowpass(this, "A_LP"), _airspeedDerivative(this, "AD"), _throttleSp(0.0f), @@ -93,18 +94,22 @@ int mTecs::updateAltitudeSpeed(float flightPathAngle, float altitude, float alti /* time measurement */ updateTimeMeasurement(); + /* Filter altitude */ + float altitudeFiltered = _altitudeLowpass.update(altitude); + + /* calculate flight path angle setpoint from altitude setpoint */ - float flightPathAngleSp = _controlAltitude.update(altitudeSp - altitude); + float flightPathAngleSp = _controlAltitude.update(altitudeSp - altitudeFiltered); /* Debug output */ if (_counter % 10 == 0) { debug("***"); - debug("updateAltitudeSpeed: altitudeSp %.4f, altitude %.4f, flightPathAngleSp %.4f", (double)altitudeSp, (double)altitude, (double)flightPathAngleSp); + debug("updateAltitudeSpeed: altitudeSp %.4f, altitude %.4f, altitude filtered %.4f, flightPathAngleSp %.4f", (double)altitudeSp, (double)altitude, (double)altitudeFiltered, (double)flightPathAngleSp); } /* Write part of the status message */ _status.altitudeSp = altitudeSp; - _status.altitude = altitude; + _status.altitude_filtered = altitudeFiltered; /* use flightpath angle setpoint for total energy control */ @@ -140,8 +145,7 @@ int mTecs::updateFlightPathAngleSpeed(float flightPathAngle, float flightPathAng /* Write part of the status message */ _status.airspeedSp = airspeedSp; - _status.airspeed = airspeed; - _status.airspeedFiltered = airspeedFiltered; + _status.airspeed_filtered = airspeedFiltered; /* use longitudinal acceleration setpoint for total energy control */ @@ -220,7 +224,7 @@ int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flight /* Apply overrride given by the limitOverride argument (this is used for limits which are not given by * parameters such as pitch limits with takeoff waypoints or throttle limits when the launchdetector * is running) */ - bool limitApplied = limitOverride.applyOverride(*outputLimiterThrottle, *outputLimiterPitch); + limitOverride.applyOverride(*outputLimiterThrottle, *outputLimiterPitch); /* Write part of the status message */ _status.flightPathAngleSp = flightPathAngleSp; diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.h b/src/modules/fw_pos_control_l1/mtecs/mTecs.h index efa89a5d3..ae6867d38 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs.h +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.h @@ -115,6 +115,7 @@ protected: /* Other calculation Blocks */ control::BlockLowPass _flightPathAngleLowpass; /**< low pass filter for the flight path angle */ + control::BlockLowPass _altitudeLowpass; /**< low pass filter for altitude */ control::BlockLowPass _airspeedLowpass; /**< low pass filter for airspeed */ control::BlockDerivative _airspeedDerivative; /**< airspeed derivative calulation */ 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..bb2065472 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") {}; @@ -72,8 +72,8 @@ public: * @return: true if the limit is applied, false otherwise */ bool limit(float& value, float& difference) { - float minimum = isAngularLimit() ? getMin() * M_DEG_TO_RAD_F : getMin(); - float maximum = isAngularLimit() ? getMax() * M_DEG_TO_RAD_F : getMax(); + float minimum = getIsAngularLimit() ? getMin() * M_DEG_TO_RAD_F : getMin(); + float maximum = getIsAngularLimit() ? getMax() * M_DEG_TO_RAD_F : getMax(); if (value < minimum) { difference = value - minimum; value = minimum; @@ -86,7 +86,7 @@ public: return false; } //accessor: - bool isAngularLimit() {return _isAngularLimit ;} + bool getIsAngularLimit() {return _isAngularLimit ;} float getMin() { return _min.get(); } float getMax() { return _max.get(); } void setMin(float value) { _min.set(value); } diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c b/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c index 5b9238780..58a1e9f6b 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c @@ -55,7 +55,7 @@ * @max 1 * @group mTECS */ -PARAM_DEFINE_INT32(MT_ENABLED, 1); +PARAM_DEFINE_INT32(MT_ENABLED, 0); /** * Total Energy Rate Control Feedforward @@ -175,6 +175,13 @@ PARAM_DEFINE_FLOAT(MT_PIT_MIN, -45.0f); PARAM_DEFINE_FLOAT(MT_PIT_MAX, 20.0f); /** + * Lowpass (cutoff freq.) for altitude + * + * @group mTECS + */ +PARAM_DEFINE_FLOAT(MT_ALT_LP, 1.0f); + +/** * Lowpass (cutoff freq.) for the flight path angle * * @group mTECS @@ -234,7 +241,14 @@ PARAM_DEFINE_FLOAT(MT_FPA_MAX, 30.0f); * * @group mTECS */ -PARAM_DEFINE_FLOAT(MT_A_LP, 1.0f); +PARAM_DEFINE_FLOAT(MT_A_LP, 0.5f); + +/** + * Airspeed derivative calculation lowpass + * + * @group mTECS + */ +PARAM_DEFINE_FLOAT(MT_AD_LP, 0.5f); /** * P gain for the airspeed control @@ -261,7 +275,7 @@ PARAM_DEFINE_FLOAT(MT_ACC_D, 0.0f); * * @group mTECS */ -PARAM_DEFINE_FLOAT(MT_ACC_D_LP, 1.0f); +PARAM_DEFINE_FLOAT(MT_ACC_D_LP, 0.5f); /** * Minimal acceleration (air) @@ -280,13 +294,6 @@ PARAM_DEFINE_FLOAT(MT_ACC_MIN, -40.0f); PARAM_DEFINE_FLOAT(MT_ACC_MAX, 40.0f); /** - * Airspeed derivative calculation lowpass - * - * @group mTECS - */ -PARAM_DEFINE_FLOAT(MT_AD_LP, 1.0f); - -/** * Minimal throttle during takeoff * * @min 0.0f diff --git a/src/modules/mavlink/mavlink_bridge_header.h b/src/modules/mavlink/mavlink_bridge_header.h index 374d1511c..0cd08769e 100644 --- a/src/modules/mavlink/mavlink_bridge_header.h +++ b/src/modules/mavlink/mavlink_bridge_header.h @@ -44,7 +44,12 @@ __BEGIN_DECLS -#define MAVLINK_USE_CONVENIENCE_FUNCTIONS +/* + * We are NOT using convenience functions, + * but instead send messages with a custom function. + * So we do NOT do this: + * #define MAVLINK_USE_CONVENIENCE_FUNCTIONS + */ /* use efficient approach, see mavlink_helpers.h */ #define MAVLINK_SEND_UART_BYTES mavlink_send_uart_bytes diff --git a/src/modules/mavlink/mavlink_ftp.cpp b/src/modules/mavlink/mavlink_ftp.cpp index ca846a465..17d1babff 100644 --- a/src/modules/mavlink/mavlink_ftp.cpp +++ b/src/modules/mavlink/mavlink_ftp.cpp @@ -35,6 +35,7 @@ #include <unistd.h> #include <stdio.h> #include <fcntl.h> +#include <sys/stat.h> #include "mavlink_ftp.h" @@ -50,16 +51,20 @@ MavlinkFTP::getServer() return _server; } -MavlinkFTP::MavlinkFTP() +MavlinkFTP::MavlinkFTP() : + _session_fds{}, + _workBufs{}, + _workFree{}, + _lock{} { // initialise the request freelist dq_init(&_workFree); sem_init(&_lock, 0, 1); - - // initialize session list - for (size_t i=0; i<kMaxSession; i++) { - _session_fds[i] = -1; - } + + // initialize session list + for (size_t i=0; i<kMaxSession; i++) { + _session_fds[i] = -1; + } // drop work entries onto the free list for (unsigned i = 0; i < kRequestQueueSize; i++) { @@ -117,10 +122,10 @@ MavlinkFTP::_worker(Request *req) if (crc32(req->rawData(), req->dataSize()) != messageCRC) { errorCode = kErrNoRequest; goto out; - printf("ftp: bad crc\n"); + warnx("ftp: bad crc"); } - printf("ftp: channel %u opc %u size %u offset %u\n", req->channel(), hdr->opcode, hdr->size, hdr->offset); + //printf("ftp: channel %u opc %u size %u offset %u\n", req->channel(), hdr->opcode, hdr->size, hdr->offset); switch (hdr->opcode) { case kCmdNone: @@ -167,9 +172,9 @@ out: // handle success vs. error if (errorCode == kErrNone) { hdr->opcode = kRspAck; - printf("FTP: ack\n"); + //warnx("FTP: ack\n"); } else { - printf("FTP: nak %u\n", errorCode); + warnx("FTP: nak %u", errorCode); hdr->opcode = kRspNak; hdr->size = 1; hdr->data[0] = errorCode; @@ -186,6 +191,8 @@ void MavlinkFTP::_reply(Request *req) { auto hdr = req->header(); + + hdr->magic = kProtocolMagic; // message is assumed to be already constructed in the request buffer, so generate the CRC hdr->crc32 = 0; @@ -199,12 +206,18 @@ MavlinkFTP::ErrorCode MavlinkFTP::_workList(Request *req) { auto hdr = req->header(); - DIR *dp = opendir(req->dataAsCString()); + + char dirPath[kMaxDataLength]; + strncpy(dirPath, req->dataAsCString(), kMaxDataLength); + + DIR *dp = opendir(dirPath); if (dp == nullptr) { - printf("FTP: can't open path '%s'\n", req->dataAsCString()); + warnx("FTP: can't open path '%s'", dirPath); return kErrNotDir; } + + //warnx("FTP: list %s offset %d", dirPath, hdr->offset); ErrorCode errorCode = kErrNone; struct dirent entry, *result = nullptr; @@ -216,6 +229,7 @@ MavlinkFTP::_workList(Request *req) for (;;) { // read the directory entry if (readdir_r(dp, &entry, &result)) { + warnx("FTP: list %s readdir_r failure\n", dirPath); errorCode = kErrIO; break; } @@ -231,15 +245,18 @@ MavlinkFTP::_workList(Request *req) break; } - // name too big to fit? - if ((strlen(entry.d_name) + offset + 2) > kMaxDataLength) { - break; - } + uint32_t fileSize = 0; + char buf[256]; // store the type marker switch (entry.d_type) { case DTYPE_FILE: hdr->data[offset++] = kDirentFile; + snprintf(buf, sizeof(buf), "%s/%s", dirPath, entry.d_name); + struct stat st; + if (stat(buf, &st) == 0) { + fileSize = st.st_size; + } break; case DTYPE_DIRECTORY: hdr->data[offset++] = kDirentDir; @@ -248,11 +265,24 @@ MavlinkFTP::_workList(Request *req) hdr->data[offset++] = kDirentUnknown; break; } + + if (entry.d_type == DTYPE_FILE) { + snprintf(buf, sizeof(buf), "%s\t%d", entry.d_name, fileSize); + } else { + strncpy(buf, entry.d_name, sizeof(buf)); + buf[sizeof(buf)-1] = 0; + } + size_t nameLen = strlen(buf); + // name too big to fit? + if ((nameLen + offset + 2) > kMaxDataLength) { + break; + } + // copy the name, which we know will fit - strcpy((char *)&hdr->data[offset], entry.d_name); - offset += strlen(entry.d_name) + 1; - printf("FTP: list %s\n", entry.d_name); + strcpy((char *)&hdr->data[offset], buf); + //printf("FTP: list %s %s\n", dirPath, (char *)&hdr->data[offset-1]); + offset += nameLen + 1; } closedir(dp); @@ -271,6 +301,16 @@ MavlinkFTP::_workOpen(Request *req, bool create) return kErrNoSession; } + + uint32_t fileSize = 0; + if (!create) { + struct stat st; + if (stat(req->dataAsCString(), &st) != 0) { + return kErrNotFile; + } + fileSize = st.st_size; + } + int oflag = create ? (O_CREAT | O_EXCL | O_APPEND) : O_RDONLY; int fd = ::open(req->dataAsCString(), oflag); @@ -280,7 +320,12 @@ MavlinkFTP::_workOpen(Request *req, bool create) _session_fds[session_index] = fd; hdr->session = session_index; - hdr->size = 0; + if (create) { + hdr->size = 0; + } else { + hdr->size = sizeof(uint32_t); + *((uint32_t*)hdr->data) = fileSize; + } return kErrNone; } @@ -297,19 +342,20 @@ MavlinkFTP::_workRead(Request *req) } // Seek to the specified position - printf("Seek %d\n", hdr->offset); + //warnx("seek %d", hdr->offset); if (lseek(_session_fds[session_index], hdr->offset, SEEK_SET) < 0) { // Unable to see to the specified location + warnx("seek fail"); return kErrEOF; } int bytes_read = ::read(_session_fds[session_index], &hdr->data[0], kMaxDataLength); if (bytes_read < 0) { // Negative return indicates error other than eof + warnx("read fail %d", bytes_read); return kErrIO; } - printf("Read success %d\n", bytes_read); hdr->size = bytes_read; return kErrNone; @@ -346,7 +392,7 @@ MavlinkFTP::_workWrite(Request *req) MavlinkFTP::ErrorCode MavlinkFTP::_workRemove(Request *req) { - auto hdr = req->header(); + //auto hdr = req->header(); // for now, send error reply return kErrPerm; diff --git a/src/modules/mavlink/mavlink_ftp.h b/src/modules/mavlink/mavlink_ftp.h index e22e61553..800c98b69 100644 --- a/src/modules/mavlink/mavlink_ftp.h +++ b/src/modules/mavlink/mavlink_ftp.h @@ -55,6 +55,7 @@ #include <systemlib/err.h> #include "mavlink_messages.h" +#include "mavlink_main.h" class MavlinkFTP { @@ -145,22 +146,23 @@ private: mavlink_message_t msg; msg.checksum = 0; unsigned len = mavlink_msg_encapsulated_data_pack_chan(_mavlink->get_system_id(), _mavlink->get_component_id(), - _mavlink->get_channel(), &msg, sequence(), rawData()); + _mavlink->get_channel(), &msg, sequence()+1, rawData()); - _mavlink->lockMessageBufferMutex(); - bool fError = _mavlink->message_buffer_write(&msg, len); - _mavlink->unlockMessageBufferMutex(); - - if (!fError) { + _mavlink->lockMessageBufferMutex(); + bool success = _mavlink->message_buffer_write(&msg, len); + _mavlink->unlockMessageBufferMutex(); + + if (!success) { warnx("FTP TX ERR"); - } else { - warnx("wrote: sys: %d, comp: %d, chan: %d, len: %d, checksum: %d", - _mavlink->get_system_id(), - _mavlink->get_component_id(), - _mavlink->get_channel(), - len, - msg.checksum); - } + } + // else { + // warnx("wrote: sys: %d, comp: %d, chan: %d, len: %d, checksum: %d", + // _mavlink->get_system_id(), + // _mavlink->get_component_id(), + // _mavlink->get_channel(), + // len, + // msg.checksum); + // } } uint8_t *rawData() { return &_message.data[0]; } diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 026a4d6c9..9a39d3bed 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -18,7 +18,6 @@ * * 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, @@ -73,18 +72,15 @@ #include <mavlink/mavlink_log.h> #include <uORB/topics/parameter_update.h> -#include <uORB/topics/mission.h> -#include <uORB/topics/mission_result.h> #include "mavlink_bridge_header.h" #include "mavlink_main.h" #include "mavlink_messages.h" #include "mavlink_receiver.h" #include "mavlink_rate_limiter.h" -#include "mavlink_commands.h" #ifndef MAVLINK_CRC_EXTRA - #error MAVLINK_CRC_EXTRA has to be defined on PX4 systems +#error MAVLINK_CRC_EXTRA has to be defined on PX4 systems #endif /* oddly, ERROR is not defined for c++ */ @@ -94,14 +90,19 @@ static const int ERROR = -1; #define DEFAULT_DEVICE_NAME "/dev/ttyS1" -#define MAX_DATA_RATE 10000 // max data rate in bytes/s +#define MAX_DATA_RATE 20000 // max data rate in bytes/s #define MAIN_LOOP_DELAY 10000 // 100 Hz @ 1000 bytes/s data rate +#define TX_BUFFER_GAP MAVLINK_MAX_PACKET_LEN + static Mavlink *_mavlink_instances = nullptr; /* TODO: if this is a class member it crashes */ static struct file_operations fops; +static const uint8_t mavlink_message_lengths[256] = MAVLINK_MESSAGE_LENGTHS; +static const uint8_t mavlink_message_crcs[256] = MAVLINK_MESSAGE_CRCS; + /** * mavlink app start / stop handling function * @@ -109,110 +110,7 @@ static struct file_operations fops; */ extern "C" __EXPORT int mavlink_main(int argc, char *argv[]); -static uint64_t last_write_success_times[6] = {0}; -static uint64_t last_write_try_times[6] = {0}; - -/* - * Internal function to send the bytes through the right serial port - */ -void -mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length) -{ - - Mavlink *instance; - - switch (channel) { - case MAVLINK_COMM_0: - instance = Mavlink::get_instance(0); - break; - - case MAVLINK_COMM_1: - instance = Mavlink::get_instance(1); - break; - - case MAVLINK_COMM_2: - instance = Mavlink::get_instance(2); - break; - - case MAVLINK_COMM_3: - instance = Mavlink::get_instance(3); - break; -#ifdef MAVLINK_COMM_4 - - case MAVLINK_COMM_4: - instance = Mavlink::get_instance(4); - break; -#endif -#ifdef MAVLINK_COMM_5 - - case MAVLINK_COMM_5: - instance = Mavlink::get_instance(5); - break; -#endif -#ifdef MAVLINK_COMM_6 - - case MAVLINK_COMM_6: - instance = Mavlink::get_instance(6); - break; -#endif - default: - return; - } - - int uart = instance->get_uart_fd(); - - ssize_t desired = (sizeof(uint8_t) * length); - - /* - * Check if the OS buffer is full and disable HW - * flow control if it continues to be full - */ - int buf_free = 0; - - if (instance->get_flow_control_enabled() - && ioctl(uart, FIONWRITE, (unsigned long)&buf_free) == 0) { - - /* Disable hardware flow control: - * if no successful write since a defined time - * and if the last try was not the last successful write - */ - if (last_write_try_times[(unsigned)channel] != 0 && - hrt_elapsed_time(&last_write_success_times[(unsigned)channel]) > 500 * 1000UL && - last_write_success_times[(unsigned)channel] != - last_write_try_times[(unsigned)channel]) - { - warnx("DISABLING HARDWARE FLOW CONTROL"); - instance->enable_flow_control(false); - } - - } - - /* If the wait until transmit flag is on, only transmit after we've received messages. - Otherwise, transmit all the time. */ - if (instance->should_transmit()) { - last_write_try_times[(unsigned)channel] = hrt_absolute_time(); - - /* check if there is space in the buffer, let it overflow else */ - if (!ioctl(uart, FIONWRITE, (unsigned long)&buf_free)) { - - if (buf_free < desired) { - /* we don't want to send anything just in half, so return */ - instance->count_txerr(); - return; - } - } - - ssize_t ret = write(uart, ch, desired); - if (ret != desired) { - instance->count_txerr(); - } else { - last_write_success_times[(unsigned)channel] = last_write_try_times[(unsigned)channel]; - } - } - - - -} +extern mavlink_system_t mavlink_system; static void usage(void); @@ -220,6 +118,7 @@ Mavlink::Mavlink() : _device_name(DEFAULT_DEVICE_NAME), _task_should_exit(false), next(nullptr), + _instance_id(0), _mavlink_fd(-1), _task_running(false), _hil_enabled(false), @@ -230,31 +129,50 @@ Mavlink::Mavlink() : _main_loop_delay(1000), _subscriptions(nullptr), _streams(nullptr), - _mission_pub(-1), + _mission_manager(nullptr), + _parameters_manager(nullptr), _mode(MAVLINK_MODE_NORMAL), - _total_counter(0), - _verbose(false), - _forwarding_on(false), - _passing_on(false), - _ftp_on(false), - _uart_fd(-1), - _mavlink_param_queue_index(0), - _subscribe_to_stream(nullptr), - _subscribe_to_stream_rate(0.0f), - _flow_control_enabled(true), - _message_buffer({}), - _param_initialized(false), - _param_system_id(0), - _param_component_id(0), - _param_system_type(0), - _param_use_hil_gps(0), - -/* performance counters */ - _loop_perf(perf_alloc(PC_ELAPSED, "mavlink_el")), - _txerr_perf(perf_alloc(PC_COUNT, "mavlink_txe")) + _channel(MAVLINK_COMM_0), + _logbuffer {}, + _total_counter(0), + _receive_thread {}, + _verbose(false), + _forwarding_on(false), + _passing_on(false), + _ftp_on(false), + _uart_fd(-1), + _baudrate(57600), + _datarate(1000), + _datarate_events(500), + _rate_mult(1.0f), + _mavlink_param_queue_index(0), + mavlink_link_termination_allowed(false), + _subscribe_to_stream(nullptr), + _subscribe_to_stream_rate(0.0f), + _flow_control_enabled(true), + _last_write_success_time(0), + _last_write_try_time(0), + _bytes_tx(0), + _bytes_txerr(0), + _bytes_rx(0), + _bytes_timestamp(0), + _rate_tx(0.0f), + _rate_txerr(0.0f), + _rate_rx(0.0f), + _rstatus {}, + _message_buffer {}, + _message_buffer_mutex {}, + _send_mutex {}, + _param_initialized(false), + _param_system_id(0), + _param_component_id(0), + _param_system_type(0), + _param_use_hil_gps(0), + + /* performance counters */ + _loop_perf(perf_alloc(PC_ELAPSED, "mavlink_el")), + _txerr_perf(perf_alloc(PC_COUNT, "mavlink_txe")) { - _wpm = &_wpm_s; - mission.count = 0; fops.ioctl = (int (*)(file *, int, long unsigned int))&mavlink_dev_ioctl; _instance_id = Mavlink::instance_count(); @@ -420,6 +338,27 @@ Mavlink::destroy_all_instances() return OK; } +int +Mavlink::get_status_all_instances() +{ + Mavlink *inst = ::_mavlink_instances; + + unsigned iterations = 0; + + while (inst != nullptr) { + + printf("\ninstance #%u:\n", iterations); + inst->display_status(); + + /* move on */ + inst = inst->next; + iterations++; + } + + /* return an error if there are no instances */ + return (iterations == 0); +} + bool Mavlink::instance_exists(const char *device_name, Mavlink *self) { @@ -439,13 +378,18 @@ Mavlink::instance_exists(const char *device_name, Mavlink *self) } void -Mavlink::forward_message(mavlink_message_t *msg, Mavlink *self) +Mavlink::forward_message(const mavlink_message_t *msg, Mavlink *self) { Mavlink *inst; LL_FOREACH(_mavlink_instances, inst) { if (inst != self) { - inst->pass_message(msg); + + /* if not in normal mode, we are an onboard link + * onboard links should only pass on messages from the same system ID */ + if (!(self->_mode != MAVLINK_MODE_NORMAL && msg->sysid != mavlink_system.sysid)) { + inst->pass_message(msg); + } } } } @@ -493,10 +437,27 @@ Mavlink::mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg) case (int)MAVLINK_IOC_SEND_TEXT_EMERGENCY: { const char *txt = (const char *)arg; -// printf("logmsg: %s\n", txt); struct mavlink_logmessage msg; strncpy(msg.text, txt, sizeof(msg.text)); + switch (cmd) { + case MAVLINK_IOC_SEND_TEXT_INFO: + msg.severity = MAV_SEVERITY_INFO; + break; + + case MAVLINK_IOC_SEND_TEXT_CRITICAL: + msg.severity = MAV_SEVERITY_CRITICAL; + break; + + case MAVLINK_IOC_SEND_TEXT_EMERGENCY: + msg.severity = MAV_SEVERITY_EMERGENCY; + break; + + default: + msg.severity = MAV_SEVERITY_INFO; + break; + } + Mavlink *inst; LL_FOREACH(_mavlink_instances, inst) { if (!inst->_task_should_exit) { @@ -515,28 +476,44 @@ Mavlink::mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg) void Mavlink::mavlink_update_system(void) { - if (!_param_initialized) { _param_system_id = param_find("MAV_SYS_ID"); _param_component_id = param_find("MAV_COMP_ID"); _param_system_type = param_find("MAV_TYPE"); _param_use_hil_gps = param_find("MAV_USEHILGPS"); - _param_initialized = true; } /* update system and component id */ int32_t system_id; param_get(_param_system_id, &system_id); - if (system_id > 0 && system_id < 255) { - mavlink_system.sysid = system_id; - } - int32_t component_id; param_get(_param_component_id, &component_id); - if (component_id > 0 && component_id < 255) { - mavlink_system.compid = component_id; + + /* only allow system ID and component ID updates + * after reboot - not during operation */ + if (!_param_initialized) { + if (system_id > 0 && system_id < 255) { + mavlink_system.sysid = system_id; + } + + if (component_id > 0 && component_id < 255) { + mavlink_system.compid = component_id; + } + + _param_initialized = true; + } + + /* warn users that they need to reboot to take this + * into effect + */ + if (system_id != mavlink_system.sysid) { + send_statustext_critical("Save params and reboot to change SYSID"); + } + + if (component_id != mavlink_system.compid) { + send_statustext_critical("Save params and reboot to change COMPID"); } int32_t system_type; @@ -611,7 +588,8 @@ int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios * case 921600: speed = B921600; break; default: - warnx("ERROR: Unsupported baudrate: %d\n\tsupported examples:\n\t9600, 19200, 38400, 57600\t\n115200\n230400\n460800\n921600\n", baud); + warnx("ERROR: Unsupported baudrate: %d\n\tsupported examples:\n\t9600, 19200, 38400, 57600\t\n115200\n230400\n460800\n921600\n", + baud); return -EINVAL; } @@ -672,6 +650,9 @@ int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios * if (enable_flow_control(true)) { warnx("hardware flow control not supported"); } + + } else { + _flow_control_enabled = false; } return _uart_fd; @@ -713,8 +694,7 @@ Mavlink::set_hil_enabled(bool hil_enabled) /* enable HIL */ if (hil_enabled && !_hil_enabled) { _hil_enabled = true; - float rate_mult = _datarate / 1000.0f; - configure_stream("HIL_CONTROLS", 15.0f * rate_mult); + configure_stream("HIL_CONTROLS", 200.0f); } /* disable HIL */ @@ -729,829 +709,188 @@ Mavlink::set_hil_enabled(bool hil_enabled) return ret; } -extern mavlink_system_t mavlink_system; - -int Mavlink::mavlink_pm_queued_send() +unsigned +Mavlink::get_free_tx_buf() { - if (_mavlink_param_queue_index < param_count()) { - mavlink_pm_send_param(param_for_index(_mavlink_param_queue_index)); - _mavlink_param_queue_index++; - return 0; - - } else { - return 1; - } -} - -void Mavlink::mavlink_pm_start_queued_send() -{ - _mavlink_param_queue_index = 0; -} - -int Mavlink::mavlink_pm_send_param_for_index(uint16_t index) -{ - return mavlink_pm_send_param(param_for_index(index)); -} - -int Mavlink::mavlink_pm_send_param_for_name(const char *name) -{ - return mavlink_pm_send_param(param_find(name)); -} - -int Mavlink::mavlink_pm_send_param(param_t param) -{ - if (param == PARAM_INVALID) { return 1; } - - /* buffers for param transmission */ - char name_buf[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN]; - float val_buf; - mavlink_message_t tx_msg; - - /* query parameter type */ - param_type_t type = param_type(param); - /* copy parameter name */ - strncpy((char *)name_buf, param_name(param), MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN); - /* - * Map onboard parameter type to MAVLink type, - * endianess matches (both little endian) - */ - uint8_t mavlink_type; - - if (type == PARAM_TYPE_INT32) { - mavlink_type = MAVLINK_TYPE_INT32_T; - - } else if (type == PARAM_TYPE_FLOAT) { - mavlink_type = MAVLINK_TYPE_FLOAT; - - } else { - mavlink_type = MAVLINK_TYPE_FLOAT; - } - - /* - * get param value, since MAVLink encodes float and int params in the same - * space during transmission, copy param onto float val_buf + * Check if the OS buffer is full and disable HW + * flow control if it continues to be full */ + int buf_free = 0; + (void) ioctl(_uart_fd, FIONWRITE, (unsigned long)&buf_free); - int ret; - - if ((ret = param_get(param, &val_buf)) != OK) { - return ret; + if (get_flow_control_enabled() && buf_free < TX_BUFFER_GAP) { + /* Disable hardware flow control: + * if no successful write since a defined time + * and if the last try was not the last successful write + */ + if (_last_write_try_time != 0 && + hrt_elapsed_time(&_last_write_success_time) > 500 * 1000UL && + _last_write_success_time != _last_write_try_time) { + warnx("DISABLING HARDWARE FLOW CONTROL"); + enable_flow_control(false); + } } - mavlink_msg_param_value_pack_chan(mavlink_system.sysid, - mavlink_system.compid, - _channel, - &tx_msg, - name_buf, - val_buf, - mavlink_type, - param_count(), - param_get_index(param)); - mavlink_missionlib_send_message(&tx_msg); - return OK; -} - -void Mavlink::mavlink_pm_message_handler(const mavlink_channel_t chan, const mavlink_message_t *msg) -{ - switch (msg->msgid) { - case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: { - mavlink_param_request_list_t req; - mavlink_msg_param_request_list_decode(msg, &req); - if (req.target_system == mavlink_system.sysid && - (req.target_component == mavlink_system.compid || req.target_component == MAV_COMP_ID_ALL)) { - /* Start sending parameters */ - mavlink_pm_start_queued_send(); - mavlink_missionlib_send_gcs_string("[mavlink pm] sending list"); - } - } break; - - case MAVLINK_MSG_ID_PARAM_SET: { - - /* Handle parameter setting */ - - if (msg->msgid == MAVLINK_MSG_ID_PARAM_SET) { - mavlink_param_set_t mavlink_param_set; - mavlink_msg_param_set_decode(msg, &mavlink_param_set); - - if (mavlink_param_set.target_system == mavlink_system.sysid && ((mavlink_param_set.target_component == mavlink_system.compid) || (mavlink_param_set.target_component == MAV_COMP_ID_ALL))) { - /* local name buffer to enforce null-terminated string */ - char name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN + 1]; - strncpy(name, mavlink_param_set.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN); - /* enforce null termination */ - name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN] = '\0'; - /* attempt to find parameter, set and send it */ - param_t param = param_find(name); - - if (param == PARAM_INVALID) { - char buf[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN]; - sprintf(buf, "[pm] unknown: %s", name); - mavlink_missionlib_send_gcs_string(buf); - - } else { - /* set and send parameter */ - param_set(param, &(mavlink_param_set.param_value)); - mavlink_pm_send_param(param); - } - } - } - } break; - - case MAVLINK_MSG_ID_PARAM_REQUEST_READ: { - mavlink_param_request_read_t mavlink_param_request_read; - mavlink_msg_param_request_read_decode(msg, &mavlink_param_request_read); - - if (mavlink_param_request_read.target_system == mavlink_system.sysid && ((mavlink_param_request_read.target_component == mavlink_system.compid) || (mavlink_param_request_read.target_component == MAV_COMP_ID_ALL))) { - /* when no index is given, loop through string ids and compare them */ - if (mavlink_param_request_read.param_index == -1) { - /* local name buffer to enforce null-terminated string */ - char name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN + 1]; - strncpy(name, mavlink_param_request_read.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN); - /* enforce null termination */ - name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN] = '\0'; - /* attempt to find parameter and send it */ - mavlink_pm_send_param_for_name(name); - - } else { - /* when index is >= 0, send this parameter again */ - mavlink_pm_send_param_for_index(mavlink_param_request_read.param_index); - } - } - - } break; - } + return buf_free; } -void Mavlink::publish_mission() +void +Mavlink::send_message(const uint8_t msgid, const void *msg) { - /* Initialize mission publication if necessary */ - if (_mission_pub < 0) { - _mission_pub = orb_advertise(ORB_ID(offboard_mission), &mission); - - } else { - orb_publish(ORB_ID(offboard_mission), _mission_pub, &mission); + /* If the wait until transmit flag is on, only transmit after we've received messages. + Otherwise, transmit all the time. */ + if (!should_transmit()) { + return; } -} -int Mavlink::map_mavlink_mission_item_to_mission_item(const mavlink_mission_item_t *mavlink_mission_item, struct mission_item_s *mission_item) -{ - /* only support global waypoints for now */ - switch (mavlink_mission_item->frame) { - case MAV_FRAME_GLOBAL: - mission_item->lat = (double)mavlink_mission_item->x; - mission_item->lon = (double)mavlink_mission_item->y; - mission_item->altitude = mavlink_mission_item->z; - mission_item->altitude_is_relative = false; - break; + pthread_mutex_lock(&_send_mutex); - case MAV_FRAME_GLOBAL_RELATIVE_ALT: - mission_item->lat = (double)mavlink_mission_item->x; - mission_item->lon = (double)mavlink_mission_item->y; - mission_item->altitude = mavlink_mission_item->z; - mission_item->altitude_is_relative = true; - break; + int buf_free = get_free_tx_buf(); - case MAV_FRAME_LOCAL_NED: - case MAV_FRAME_LOCAL_ENU: - return MAV_MISSION_UNSUPPORTED_FRAME; + uint8_t payload_len = mavlink_message_lengths[msgid]; + unsigned packet_len = payload_len + MAVLINK_NUM_NON_PAYLOAD_BYTES; - case MAV_FRAME_MISSION: - default: - return MAV_MISSION_ERROR; - } + _last_write_try_time = hrt_absolute_time(); - switch (mavlink_mission_item->command) { - case MAV_CMD_NAV_TAKEOFF: - mission_item->pitch_min = mavlink_mission_item->param1; - break; - case MAV_CMD_DO_JUMP: - mission_item->do_jump_mission_index = mavlink_mission_item->param1; - mission_item->do_jump_current_count = 0; - mission_item->do_jump_repeat_count = mavlink_mission_item->param2; - break; - default: - mission_item->acceptance_radius = mavlink_mission_item->param2; - mission_item->time_inside = mavlink_mission_item->param1; - break; + /* check if there is space in the buffer, let it overflow else */ + if (buf_free < TX_BUFFER_GAP) { + /* no enough space in buffer to send */ + count_txerr(); + count_txerrbytes(packet_len); + pthread_mutex_unlock(&_send_mutex); + return; } - mission_item->yaw = _wrap_pi(mavlink_mission_item->param4 * M_DEG_TO_RAD_F); - mission_item->loiter_radius = fabsf(mavlink_mission_item->param3); - mission_item->loiter_direction = (mavlink_mission_item->param3 > 0) ? 1 : -1; /* 1 if positive CW, -1 if negative CCW */ - mission_item->nav_cmd = (NAV_CMD)mavlink_mission_item->command; - - mission_item->autocontinue = mavlink_mission_item->autocontinue; - // mission_item->index = mavlink_mission_item->seq; - mission_item->origin = ORIGIN_MAVLINK; + uint8_t buf[MAVLINK_MAX_PACKET_LEN]; - /* reset DO_JUMP count */ - mission_item->do_jump_current_count = 0; + /* header */ + buf[0] = MAVLINK_STX; + buf[1] = payload_len; + /* use mavlink's internal counter for the TX seq */ + buf[2] = mavlink_get_channel_status(_channel)->current_tx_seq++; + buf[3] = mavlink_system.sysid; + buf[4] = mavlink_system.compid; + buf[5] = msgid; - return OK; -} + /* payload */ + memcpy(&buf[MAVLINK_NUM_HEADER_BYTES], msg, payload_len); -int Mavlink::map_mission_item_to_mavlink_mission_item(const struct mission_item_s *mission_item, mavlink_mission_item_t *mavlink_mission_item) -{ - if (mission_item->altitude_is_relative) { - mavlink_mission_item->frame = MAV_FRAME_GLOBAL; + /* checksum */ + uint16_t checksum; + crc_init(&checksum); + crc_accumulate_buffer(&checksum, (const char *) &buf[1], MAVLINK_CORE_HEADER_LEN + payload_len); + crc_accumulate(mavlink_message_crcs[msgid], &checksum); - } else { - mavlink_mission_item->frame = MAV_FRAME_GLOBAL_RELATIVE_ALT; - } + buf[MAVLINK_NUM_HEADER_BYTES + payload_len] = (uint8_t)(checksum & 0xFF); + buf[MAVLINK_NUM_HEADER_BYTES + payload_len + 1] = (uint8_t)(checksum >> 8); - switch (mission_item->nav_cmd) { - case NAV_CMD_TAKEOFF: - mavlink_mission_item->param1 = mission_item->pitch_min; - break; + /* send message to UART */ + ssize_t ret = write(_uart_fd, buf, packet_len); - case NAV_CMD_DO_JUMP: - mavlink_mission_item->param1 = mission_item->do_jump_mission_index; - mavlink_mission_item->param2 = mission_item->do_jump_repeat_count; - break; + if (ret != (int) packet_len) { + count_txerr(); + count_txerrbytes(packet_len); - default: - mavlink_mission_item->param2 = mission_item->acceptance_radius; - mavlink_mission_item->param1 = mission_item->time_inside; - break; + } else { + _last_write_success_time = _last_write_try_time; + count_txbytes(packet_len); } - mavlink_mission_item->x = (float)mission_item->lat; - mavlink_mission_item->y = (float)mission_item->lon; - mavlink_mission_item->z = mission_item->altitude; - - mavlink_mission_item->param4 = mission_item->yaw * M_RAD_TO_DEG_F; - mavlink_mission_item->param3 = mission_item->loiter_radius * (float)mission_item->loiter_direction; - mavlink_mission_item->command = mission_item->nav_cmd; - mavlink_mission_item->autocontinue = mission_item->autocontinue; - // mavlink_mission_item->seq = mission_item->index; - - return OK; + pthread_mutex_unlock(&_send_mutex); } -void Mavlink::mavlink_wpm_init(mavlink_wpm_storage *state) -{ - state->size = 0; - state->max_size = MAVLINK_WPM_MAX_WP_COUNT; - state->current_state = MAVLINK_WPM_STATE_IDLE; - state->current_partner_sysid = 0; - state->current_partner_compid = 0; - state->timestamp_lastaction = 0; - state->timestamp_last_send_setpoint = 0; - state->timestamp_last_send_request = 0; - state->timeout = MAVLINK_WPM_PROTOCOL_TIMEOUT_DEFAULT; - state->current_dataman_id = 0; -} - -/* - * @brief Sends an waypoint ack message - */ -void Mavlink::mavlink_wpm_send_waypoint_ack(uint8_t sysid, uint8_t compid, uint8_t type) -{ - mavlink_message_t msg; - mavlink_mission_ack_t wpa; - - wpa.target_system = sysid; - wpa.target_component = compid; - wpa.type = type; - - mavlink_msg_mission_ack_encode_chan(mavlink_system.sysid, _mavlink_wpm_comp_id, _channel, &msg, &wpa); - mavlink_missionlib_send_message(&msg); - - if (_verbose) { warnx("Sent waypoint ack (%u) to ID %u", wpa.type, wpa.target_system); } -} - -/* - * @brief Broadcasts the new target waypoint and directs the MAV to fly there - * - * This function broadcasts its new active waypoint sequence number and - * sends a message to the controller, advising it to fly to the coordinates - * of the waypoint with a given orientation - * - * @param seq The waypoint sequence number the MAV should fly to. - */ -void Mavlink::mavlink_wpm_send_waypoint_current(uint16_t seq) +void +Mavlink::resend_message(mavlink_message_t *msg) { - if (seq < _wpm->size) { - mavlink_message_t msg; - mavlink_mission_current_t wpc; - - wpc.seq = seq; - - mavlink_msg_mission_current_encode_chan(mavlink_system.sysid, _mavlink_wpm_comp_id, _channel, &msg, &wpc); - mavlink_missionlib_send_message(&msg); - - } else if (seq == 0 && _wpm->size == 0) { - - /* don't broadcast if no WPs */ - - } else { - mavlink_missionlib_send_gcs_string("ERROR: wp index out of bounds"); + /* If the wait until transmit flag is on, only transmit after we've received messages. + Otherwise, transmit all the time. */ + if (!should_transmit()) { + return; } -} -void Mavlink::mavlink_wpm_send_waypoint_count(uint8_t sysid, uint8_t compid, uint16_t count) -{ - mavlink_message_t msg; - mavlink_mission_count_t wpc; - - wpc.target_system = sysid; - wpc.target_component = compid; - wpc.count = mission.count; - - mavlink_msg_mission_count_encode_chan(mavlink_system.sysid, _mavlink_wpm_comp_id, _channel, &msg, &wpc); - mavlink_missionlib_send_message(&msg); - - if (_verbose) { warnx("Sent waypoint count (%u) to ID %u", wpc.count, wpc.target_system); } -} - -void Mavlink::mavlink_wpm_send_waypoint(uint8_t sysid, uint8_t compid, uint16_t seq) -{ + pthread_mutex_lock(&_send_mutex); - struct mission_item_s mission_item; - ssize_t len = sizeof(struct mission_item_s); + int buf_free = get_free_tx_buf(); - dm_item_t dm_current; + _last_write_try_time = hrt_absolute_time(); - if (_wpm->current_dataman_id == 0) { - dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0; + unsigned packet_len = msg->len + MAVLINK_NUM_NON_PAYLOAD_BYTES; - } else { - dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1; + /* check if there is space in the buffer, let it overflow else */ + if (buf_free < TX_BUFFER_GAP) { + /* no enough space in buffer to send */ + count_txerr(); + count_txerrbytes(packet_len); + pthread_mutex_unlock(&_send_mutex); + return; } - if (dm_read(dm_current, seq, &mission_item, len) == len) { - - /* create mission_item_s from mavlink_mission_item_t */ - mavlink_mission_item_t wp; - map_mission_item_to_mavlink_mission_item(&mission_item, &wp); - - mavlink_message_t msg; - wp.target_system = sysid; - wp.target_component = compid; - wp.seq = seq; - mavlink_msg_mission_item_encode_chan(mavlink_system.sysid, _mavlink_wpm_comp_id, _channel, &msg, &wp); - mavlink_missionlib_send_message(&msg); + uint8_t buf[MAVLINK_MAX_PACKET_LEN]; - if (_verbose) { warnx("Sent waypoint %u to ID %u", wp.seq, wp.target_system); } + /* header and payload */ + memcpy(&buf[0], &msg->magic, MAVLINK_NUM_HEADER_BYTES + msg->len); - } else { - mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_ERROR); - mavlink_missionlib_send_gcs_string("#audio: Unable to read from micro SD"); + /* checksum */ + buf[MAVLINK_NUM_HEADER_BYTES + msg->len] = (uint8_t)(msg->checksum & 0xFF); + buf[MAVLINK_NUM_HEADER_BYTES + msg->len + 1] = (uint8_t)(msg->checksum >> 8); - if (_verbose) { warnx("ERROR: could not read WP%u", seq); } - } -} + /* send message to UART */ + ssize_t ret = write(_uart_fd, buf, packet_len); -void Mavlink::mavlink_wpm_send_waypoint_request(uint8_t sysid, uint8_t compid, uint16_t seq) -{ - if (seq < _wpm->max_size) { - mavlink_message_t msg; - mavlink_mission_request_t wpr; - wpr.target_system = sysid; - wpr.target_component = compid; - wpr.seq = seq; - mavlink_msg_mission_request_encode_chan(mavlink_system.sysid, _mavlink_wpm_comp_id, _channel, &msg, &wpr); - mavlink_missionlib_send_message(&msg); - _wpm->timestamp_last_send_request = hrt_absolute_time(); - - if (_verbose) { warnx("Sent waypoint request %u to ID %u", wpr.seq, wpr.target_system); } + if (ret != (int) packet_len) { + count_txerr(); + count_txerrbytes(packet_len); } else { - mavlink_missionlib_send_gcs_string("ERROR: Waypoint index exceeds list capacity"); + _last_write_success_time = _last_write_try_time; + count_txbytes(packet_len); } -} - -/* - * @brief emits a message that a waypoint reached - * - * This function broadcasts a message that a waypoint is reached. - * - * @param seq The waypoint sequence number the MAV has reached. - */ -void Mavlink::mavlink_wpm_send_waypoint_reached(uint16_t seq) -{ - mavlink_message_t msg; - mavlink_mission_item_reached_t wp_reached; - - wp_reached.seq = seq; - - mavlink_msg_mission_item_reached_encode_chan(mavlink_system.sysid, _mavlink_wpm_comp_id, _channel, &msg, &wp_reached); - mavlink_missionlib_send_message(&msg); - if (_verbose) { warnx("Sent waypoint %u reached message", wp_reached.seq); } + pthread_mutex_unlock(&_send_mutex); } -void Mavlink::mavlink_waypoint_eventloop(uint64_t now) +void +Mavlink::handle_message(const mavlink_message_t *msg) { - /* check for timed-out operations */ - if (now - _wpm->timestamp_lastaction > _wpm->timeout && _wpm->current_state != MAVLINK_WPM_STATE_IDLE) { - - mavlink_missionlib_send_gcs_string("Operation timeout"); + /* handle packet with mission manager */ + _mission_manager->handle_message(msg); - _wpm->current_state = MAVLINK_WPM_STATE_IDLE; - _wpm->current_partner_sysid = 0; - _wpm->current_partner_compid = 0; + /* handle packet with parameter component */ + _parameters_manager->handle_message(msg); - } else if (now - _wpm->timestamp_last_send_request > 500000 && _wpm->current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS) { - /* try to get WP again after short timeout */ - mavlink_wpm_send_waypoint_request(_wpm->current_partner_sysid, _wpm->current_partner_compid, _wpm->current_wp_id); + if (get_forwarding_on()) { + /* forward any messages to other mavlink instances */ + Mavlink::forward_message(msg, this); } } - -void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) +void +Mavlink::send_statustext_info(const char *string) { - uint64_t now = hrt_absolute_time(); - - switch (msg->msgid) { - - case MAVLINK_MSG_ID_MISSION_ACK: { - mavlink_mission_ack_t wpa; - mavlink_msg_mission_ack_decode(msg, &wpa); - - if ((msg->sysid == _wpm->current_partner_sysid && msg->compid == _wpm->current_partner_compid) && (wpa.target_system == mavlink_system.sysid /*&& wpa.target_component == mavlink_wpm_comp_id*/)) { - _wpm->timestamp_lastaction = now; - - if (_wpm->current_state == MAVLINK_WPM_STATE_SENDLIST || _wpm->current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS) { - if (_wpm->current_wp_id == _wpm->size - 1) { - - _wpm->current_state = MAVLINK_WPM_STATE_IDLE; - _wpm->current_wp_id = 0; - } - } - - } else { - mavlink_missionlib_send_gcs_string("REJ. WP CMD: curr partner id mismatch"); - } - - break; - } - - case MAVLINK_MSG_ID_MISSION_SET_CURRENT: { - mavlink_mission_set_current_t wpc; - mavlink_msg_mission_set_current_decode(msg, &wpc); - - if (wpc.target_system == mavlink_system.sysid /*&& wpc.target_component == mavlink_wpm_comp_id*/) { - _wpm->timestamp_lastaction = now; - - if (_wpm->current_state == MAVLINK_WPM_STATE_IDLE) { - if (wpc.seq < _wpm->size) { - - mission.current_index = wpc.seq; - publish_mission(); - - /* don't answer yet, wait for the navigator to respond, then publish the mission_result */ -// mavlink_wpm_send_waypoint_current(wpc.seq); - - } else { - mavlink_missionlib_send_gcs_string("IGN WP CURR CMD: Not in list"); - } - - } else { - mavlink_missionlib_send_gcs_string("IGN WP CURR CMD: Busy"); - } - } - - break; - } - - case MAVLINK_MSG_ID_MISSION_REQUEST_LIST: { - mavlink_mission_request_list_t wprl; - mavlink_msg_mission_request_list_decode(msg, &wprl); - - if (wprl.target_system == mavlink_system.sysid /*&& wprl.target_component == mavlink_wpm_comp_id*/) { - _wpm->timestamp_lastaction = now; - - if (_wpm->current_state == MAVLINK_WPM_STATE_IDLE || _wpm->current_state == MAVLINK_WPM_STATE_SENDLIST) { - if (_wpm->size > 0) { - - _wpm->current_state = MAVLINK_WPM_STATE_SENDLIST; - _wpm->current_wp_id = 0; - _wpm->current_partner_sysid = msg->sysid; - _wpm->current_partner_compid = msg->compid; - - } else { - if (_verbose) { warnx("No waypoints send"); } - } - - _wpm->current_count = _wpm->size; - mavlink_wpm_send_waypoint_count(msg->sysid, msg->compid, _wpm->current_count); - - } else { - mavlink_missionlib_send_gcs_string("IGN REQUEST LIST: Busy"); - } - } - - break; - } - - case MAVLINK_MSG_ID_MISSION_REQUEST: { - mavlink_mission_request_t wpr; - mavlink_msg_mission_request_decode(msg, &wpr); - - if (msg->sysid == _wpm->current_partner_sysid && msg->compid == _wpm->current_partner_compid && wpr.target_system == mavlink_system.sysid /*&& wpr.target_component == mavlink_wpm_comp_id*/) { - _wpm->timestamp_lastaction = now; - - if (wpr.seq >= _wpm->size) { - - mavlink_missionlib_send_gcs_string("REJ. WP CMD: Req. WP not in list"); - - break; - } - - /* - * Ensure that we are in the correct state and that the first request has id 0 - * and the following requests have either the last id (re-send last waypoint) or last_id+1 (next waypoint) - */ - if (_wpm->current_state == MAVLINK_WPM_STATE_SENDLIST) { - - if (wpr.seq == 0) { - if (_verbose) { warnx("Got ITEM_REQUEST of waypoint %u from %u changing to STATE_SENDLIST_SENDWPS", wpr.seq, msg->sysid); } - - _wpm->current_state = MAVLINK_WPM_STATE_SENDLIST_SENDWPS; - - } else { - mavlink_missionlib_send_gcs_string("REJ. WP CMD: First id != 0"); - - break; - } - - } else if (_wpm->current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS) { - - if (wpr.seq == _wpm->current_wp_id) { - - if (_verbose) { warnx("Got ITEM_REQUEST of waypoint %u (again) from %u staying in STATE_SENDLIST_SENDWPS", wpr.seq, msg->sysid); } - - } else if (wpr.seq == _wpm->current_wp_id + 1) { - - if (_verbose) { warnx("Got ITEM_REQUEST of waypoint %u from %u staying in STATE_SENDLIST_SENDWPS", wpr.seq, msg->sysid); } - - } else { - mavlink_missionlib_send_gcs_string("REJ. WP CMD: Req. WP was unexpected"); - - break; - } - - } else { - - mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy"); - - break; - } - - _wpm->current_wp_id = wpr.seq; - _wpm->current_state = MAVLINK_WPM_STATE_SENDLIST_SENDWPS; - - if (wpr.seq < _wpm->size) { - - mavlink_wpm_send_waypoint(_wpm->current_partner_sysid, _wpm->current_partner_compid, _wpm->current_wp_id); - - } else { - mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_ERROR); - - mavlink_missionlib_send_gcs_string("ERROR: Waypoint out of bounds"); - } - - - } else { - //we we're target but already communicating with someone else - if ((wpr.target_system == mavlink_system.sysid /*&& wpr.target_component == mavlink_wpm_comp_id*/) && !(msg->sysid == _wpm->current_partner_sysid && msg->compid == _wpm->current_partner_compid)) { - - mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy"); - - if (_verbose) { warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST from ID %u because i'm already talking to ID %u.", msg->sysid, _wpm->current_partner_sysid); } - } - } - - break; - } - - case MAVLINK_MSG_ID_MISSION_COUNT: { - mavlink_mission_count_t wpc; - mavlink_msg_mission_count_decode(msg, &wpc); - - if (wpc.target_system == mavlink_system.sysid/* && wpc.target_component == mavlink_wpm_comp_id*/) { - _wpm->timestamp_lastaction = now; - - if (_wpm->current_state == MAVLINK_WPM_STATE_IDLE) { - - if (wpc.count > NUM_MISSIONS_SUPPORTED) { - if (_verbose) { warnx("Too many waypoints: %d, supported: %d", wpc.count, NUM_MISSIONS_SUPPORTED); } - - mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_NO_SPACE); - break; - } - - if (wpc.count == 0) { - mavlink_missionlib_send_gcs_string("WP COUNT 0"); - - break; - } - - _wpm->current_state = MAVLINK_WPM_STATE_GETLIST; - _wpm->current_wp_id = 0; - _wpm->current_partner_sysid = msg->sysid; - _wpm->current_partner_compid = msg->compid; - _wpm->current_count = wpc.count; - - mavlink_wpm_send_waypoint_request(_wpm->current_partner_sysid, _wpm->current_partner_compid, _wpm->current_wp_id); - - } else if (_wpm->current_state == MAVLINK_WPM_STATE_GETLIST) { - - if (_wpm->current_wp_id == 0) { - mavlink_missionlib_send_gcs_string("WP CMD OK AGAIN"); - - } else { - mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy with WP"); - } - - } else { - mavlink_missionlib_send_gcs_string("IGN MISSION_COUNT CMD: Busy"); - } - } - } - break; - - case MAVLINK_MSG_ID_MISSION_ITEM: { - mavlink_mission_item_t wp; - mavlink_msg_mission_item_decode(msg, &wp); - - if (wp.target_system == mavlink_system.sysid && wp.target_component == _mavlink_wpm_comp_id) { - - _wpm->timestamp_lastaction = now; - - /* - * ensure that we are in the correct state and that the first waypoint has id 0 - * and the following waypoints have the correct ids - */ - - if (_wpm->current_state == MAVLINK_WPM_STATE_GETLIST) { - - if (wp.seq != 0) { - mavlink_missionlib_send_gcs_string("Ignored MISSION_ITEM WP not 0"); - break; - } - - } else if (_wpm->current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS) { - - if (wp.seq >= _wpm->current_count) { - mavlink_missionlib_send_gcs_string("Ignored MISSION_ITEM WP out of bounds"); - break; - } - - if (wp.seq != _wpm->current_wp_id) { - mavlink_missionlib_send_gcs_string("IGN: waypoint ID mismatch"); - mavlink_wpm_send_waypoint_request(_wpm->current_partner_sysid, _wpm->current_partner_compid, _wpm->current_wp_id); - break; - } - } - - _wpm->current_state = MAVLINK_WPM_STATE_GETLIST_GETWPS; - - struct mission_item_s mission_item; - - int ret = map_mavlink_mission_item_to_mission_item(&wp, &mission_item); - - if (ret != OK) { - mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, ret); - _wpm->current_state = MAVLINK_WPM_STATE_IDLE; - break; - } - - ssize_t len = sizeof(struct mission_item_s); - - dm_item_t dm_next; - - if (_wpm->current_dataman_id == 0) { - dm_next = DM_KEY_WAYPOINTS_OFFBOARD_1; - mission.dataman_id = 1; - - } else { - dm_next = DM_KEY_WAYPOINTS_OFFBOARD_0; - mission.dataman_id = 0; - } - - if (dm_write(dm_next, 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); - mavlink_missionlib_send_gcs_string("#audio: Unable to write on micro SD"); - _wpm->current_state = MAVLINK_WPM_STATE_IDLE; - break; - } - -// if (wp.current) { -// warnx("current is: %d", wp.seq); -// mission.current_index = wp.seq; -// } - // XXX ignore current set - mission.current_index = -1; - - _wpm->current_wp_id = wp.seq + 1; - - if (_wpm->current_wp_id == _wpm->current_count && _wpm->current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS) { - - if (_verbose) { warnx("Got all %u waypoints, changing state to MAVLINK_WPM_STATE_IDLE", _wpm->current_count); } - - mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_ACCEPTED); - - mission.count = _wpm->current_count; - - publish_mission(); - - _wpm->current_dataman_id = mission.dataman_id; - _wpm->size = _wpm->current_count; - - _wpm->current_state = MAVLINK_WPM_STATE_IDLE; - - } else { - mavlink_wpm_send_waypoint_request(_wpm->current_partner_sysid, _wpm->current_partner_compid, _wpm->current_wp_id); - } - } - - break; - } - - case MAVLINK_MSG_ID_MISSION_CLEAR_ALL: { - mavlink_mission_clear_all_t wpca; - mavlink_msg_mission_clear_all_decode(msg, &wpca); - - if (wpca.target_system == mavlink_system.sysid /*&& wpca.target_component == mavlink_wpm_comp_id */) { - - if (_wpm->current_state == MAVLINK_WPM_STATE_IDLE) { - _wpm->timestamp_lastaction = now; - - _wpm->size = 0; - - /* prepare mission topic */ - mission.dataman_id = -1; - mission.count = 0; - mission.current_index = -1; - publish_mission(); - - if (dm_clear(DM_KEY_WAYPOINTS_OFFBOARD_0) == OK && dm_clear(DM_KEY_WAYPOINTS_OFFBOARD_1) == 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); - } - - - } else { - mavlink_missionlib_send_gcs_string("IGN WP CLEAR CMD: Busy"); - - if (_verbose) { warnx("IGN WP CLEAR CMD: Busy"); } - } - } - - break; - } - - default: { - /* other messages might should get caught by mavlink and others */ - break; - } - } + send_statustext(MAV_SEVERITY_INFO, string); } void -Mavlink::mavlink_missionlib_send_message(mavlink_message_t *msg) +Mavlink::send_statustext_critical(const char *string) { - uint8_t buf[MAVLINK_MAX_PACKET_LEN]; - - uint16_t len = mavlink_msg_to_send_buffer(buf, msg); - mavlink_send_uart_bytes(_channel, buf, len); + send_statustext(MAV_SEVERITY_CRITICAL, string); } - - -int -Mavlink::mavlink_missionlib_send_gcs_string(const char *string) +void +Mavlink::send_statustext_emergency(const char *string) { - const int len = MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN; - mavlink_statustext_t statustext; - statustext.severity = MAV_SEVERITY_INFO; - - int i = 0; - - while (i < len - 1) { - statustext.text[i] = string[i]; - - if (string[i] == '\0') { - break; - } - - i++; - } - - if (i > 1) { - /* Enforce null termination */ - statustext.text[i] = '\0'; + send_statustext(MAV_SEVERITY_EMERGENCY, string); +} - mavlink_msg_statustext_send(_channel, statustext.severity, statustext.text); - return OK; +void +Mavlink::send_statustext(unsigned char severity, const char *string) +{ + struct mavlink_logmessage logmsg; + strncpy(logmsg.text, string, sizeof(logmsg.text)); + logmsg.severity = severity; - } else { - return 1; - } + mavlink_logbuffer_write(&_logbuffer, &logmsg); } MavlinkOrbSubscription *Mavlink::add_orb_subscription(const orb_id_t topic) @@ -1574,11 +913,17 @@ MavlinkOrbSubscription *Mavlink::add_orb_subscription(const orb_id_t topic) return sub_new; } +unsigned int +Mavlink::interval_from_rate(float rate) +{ + return (rate > 0.0f) ? (1000000.0f / rate) : 0; +} + int Mavlink::configure_stream(const char *stream_name, const float rate) { /* calculate interval in us, 0 means disabled stream */ - unsigned int interval = (rate > 0.0f) ? (1000000.0f / rate) : 0; + unsigned int interval = interval_from_rate(rate); /* search if stream exists */ MavlinkStream *stream; @@ -1609,10 +954,8 @@ Mavlink::configure_stream(const char *stream_name, const float rate) if (strcmp(stream_name, streams_list[i]->get_name()) == 0) { /* create new instance */ - stream = streams_list[i]->new_instance(); - stream->set_channel(get_channel()); + stream = streams_list[i]->new_instance(this); stream->set_interval(interval); - stream->subscribe(this); LL_APPEND(_streams, stream); return OK; @@ -1626,6 +969,31 @@ Mavlink::configure_stream(const char *stream_name, const float rate) } void +Mavlink::adjust_stream_rates(const float multiplier) +{ + /* do not allow to push us to zero */ + if (multiplier < 0.01f) { + return; + } + + /* search if stream exists */ + MavlinkStream *stream; + LL_FOREACH(_streams, stream) { + /* set new interval */ + unsigned interval = stream->get_interval(); + interval /= multiplier; + + /* allow max ~600 Hz */ + if (interval < 1600) { + interval = 1600; + } + + /* set new interval */ + stream->set_interval(interval * multiplier); + } +} + +void Mavlink::configure_stream_threadsafe(const char *stream_name, const float rate) { /* orb subscription must be done from the main thread, @@ -1660,12 +1028,14 @@ Mavlink::message_buffer_init(int size) _message_buffer.size = size; _message_buffer.write_ptr = 0; _message_buffer.read_ptr = 0; - _message_buffer.data = (char*)malloc(_message_buffer.size); + _message_buffer.data = (char *)malloc(_message_buffer.size); int ret; + if (_message_buffer.data == 0) { ret = ERROR; _message_buffer.size = 0; + } else { ret = OK; } @@ -1702,7 +1072,7 @@ Mavlink::message_buffer_is_empty() bool -Mavlink::message_buffer_write(void *ptr, int size) +Mavlink::message_buffer_write(const void *ptr, int size) { // bytes available to write int available = _message_buffer.read_ptr - _message_buffer.write_ptr - 1; @@ -1769,7 +1139,7 @@ Mavlink::message_buffer_mark_read(int n) } void -Mavlink::pass_message(mavlink_message_t *msg) +Mavlink::pass_message(const mavlink_message_t *msg) { if (_passing_on) { /* size is 8 bytes plus variable payload */ @@ -1780,7 +1150,31 @@ Mavlink::pass_message(mavlink_message_t *msg) } } +float +Mavlink::get_rate_mult() +{ + return _rate_mult; +} + +void +Mavlink::update_rate_mult() +{ + float const_rate = 0.0f; + float rate = 0.0f; + + MavlinkStream *stream; + LL_FOREACH(_streams, stream) { + if (stream->const_rate()) { + const_rate += stream->get_size() * 1000000.0f / stream->get_interval(); + + } else { + rate += stream->get_size() * 1000000.0f / stream->get_interval(); + } + } + /* don't scale up rates, only scale down if needed */ + _rate_mult = fminf(1.0f, ((float)_datarate - const_rate) / rate); +} int Mavlink::task_main(int argc, char *argv[]) @@ -1902,8 +1296,6 @@ Mavlink::task_main(int argc, char *argv[]) break; } - _mavlink_wpm_comp_id = MAV_COMP_ID_MISSIONPLANNER; - warnx("data rate: %d Bytes/s, port: %s, baud: %d", _datarate, _device_name, _baudrate); /* flush stdout in case MAVLink is about to take it over */ @@ -1919,6 +1311,9 @@ Mavlink::task_main(int argc, char *argv[]) return ERROR; } + /* initialize send mutex */ + pthread_mutex_init(&_send_mutex, NULL); + /* initialize mavlink text message buffering */ mavlink_logbuffer_init(&_logbuffer, 5); @@ -1928,7 +1323,7 @@ Mavlink::task_main(int argc, char *argv[]) * make space for two messages plus off-by-one space as we use the empty element * marker ring buffer approach. */ - if (OK != message_buffer_init(2 * MAVLINK_MAX_PACKET_LEN + 2)) { + if (OK != message_buffer_init(2 * sizeof(mavlink_message_t) + 1)) { errx(1, "can't allocate message buffer, exiting"); } @@ -1948,13 +1343,6 @@ Mavlink::task_main(int argc, char *argv[]) /* start the MAVLink receiver */ _receive_thread = MavlinkReceiver::receive_start(this); - /* initialize waypoint manager */ - mavlink_wpm_init(_wpm); - - int mission_result_sub = orb_subscribe(ORB_ID(mission_result)); - struct mission_result_s mission_result; - memset(&mission_result, 0, sizeof(mission_result)); - _task_running = true; MavlinkOrbSubscription *param_sub = add_orb_subscription(ORB_ID(parameter_update)); @@ -1965,34 +1353,50 @@ Mavlink::task_main(int argc, char *argv[]) struct vehicle_status_s status; status_sub->update(&status_time, &status); - MavlinkCommandsStream commands_stream(this, _channel); - - /* add default streams depending on mode and intervals depending on datarate */ - float rate_mult = _datarate / 1000.0f; + /* add default streams depending on mode */ + /* HEARTBEAT is constant rate stream, rate never adjusted */ configure_stream("HEARTBEAT", 1.0f); + /* STATUSTEXT stream is like normal stream but gets messages from logbuffer instead of uORB */ + configure_stream("STATUSTEXT", 20.0f); + + /* COMMAND_LONG stream: use high rate to avoid commands skipping */ + configure_stream("COMMAND_LONG", 100.0f); + + /* PARAM_VALUE stream */ + _parameters_manager = (MavlinkParametersManager *) MavlinkParametersManager::new_instance(this); + _parameters_manager->set_interval(interval_from_rate(30.0f)); + LL_APPEND(_streams, _parameters_manager); + + /* MISSION_STREAM stream, actually sends all MISSION_XXX messages at some rate depending on + * remote requests rate. Rate specified here controls how much bandwidth we will reserve for + * mission messages. */ + _mission_manager = (MavlinkMissionManager *) MavlinkMissionManager::new_instance(this); + _mission_manager->set_interval(interval_from_rate(10.0f)); + _mission_manager->set_verbose(_verbose); + LL_APPEND(_streams, _mission_manager); + switch (_mode) { case MAVLINK_MODE_NORMAL: configure_stream("SYS_STATUS", 1.0f); configure_stream("GPS_GLOBAL_ORIGIN", 0.5f); - configure_stream("HIGHRES_IMU", 1.0f * rate_mult); - configure_stream("ATTITUDE", 10.0f * rate_mult); - configure_stream("VFR_HUD", 10.0f * rate_mult); - configure_stream("GPS_RAW_INT", 1.0f * rate_mult); - configure_stream("GLOBAL_POSITION_INT", 3.0f * rate_mult); - configure_stream("LOCAL_POSITION_NED", 3.0f * rate_mult); - configure_stream("RC_CHANNELS_RAW", 1.0f * rate_mult); - configure_stream("NAMED_VALUE_FLOAT", 1.0f * rate_mult); - configure_stream("GLOBAL_POSITION_SETPOINT_INT", 3.0f * rate_mult); - configure_stream("ROLL_PITCH_YAW_THRUST_SETPOINT", 3.0f * rate_mult); + configure_stream("HIGHRES_IMU", 1.0f); + configure_stream("ATTITUDE", 10.0f); + configure_stream("VFR_HUD", 8.0f); + configure_stream("GPS_RAW_INT", 1.0f); + configure_stream("GLOBAL_POSITION_INT", 3.0f); + configure_stream("LOCAL_POSITION_NED", 3.0f); + configure_stream("RC_CHANNELS_RAW", 1.0f); + configure_stream("GLOBAL_POSITION_SETPOINT_INT", 3.0f); + configure_stream("ROLL_PITCH_YAW_THRUST_SETPOINT", 3.0f); configure_stream("DISTANCE_SENSOR", 0.5f); break; case MAVLINK_MODE_CAMERA: configure_stream("SYS_STATUS", 1.0f); - configure_stream("ATTITUDE", 15.0f * rate_mult); - configure_stream("GLOBAL_POSITION_INT", 15.0f * rate_mult); + configure_stream("ATTITUDE", 15.0f); + configure_stream("GLOBAL_POSITION_INT", 15.0f); configure_stream("CAMERA_CAPTURE", 1.0f); break; @@ -2000,14 +1404,8 @@ Mavlink::task_main(int argc, char *argv[]) break; } - /* don't send parameters on startup without request */ - _mavlink_param_queue_index = param_count(); - - MavlinkRateLimiter slow_rate_limiter(2000000.0f / rate_mult); - MavlinkRateLimiter fast_rate_limiter(30000.0f / rate_mult); - /* set main loop delay depending on data rate to minimize CPU overhead */ - _main_loop_delay = MAIN_LOOP_DELAY / rate_mult; + _main_loop_delay = MAIN_LOOP_DELAY * 1000 / _datarate; /* now the instance is fully initialized and we can bump the instance count */ LL_APPEND(_mavlink_instances, this); @@ -2020,6 +1418,8 @@ Mavlink::task_main(int argc, char *argv[]) hrt_abstime t = hrt_absolute_time(); + update_rate_mult(); + if (param_sub->update(¶m_time, nullptr)) { /* parameters updated */ mavlink_update_system(); @@ -2030,14 +1430,12 @@ Mavlink::task_main(int argc, char *argv[]) set_hil_enabled(status.hil_state == HIL_STATE_ON); } - /* update commands stream */ - commands_stream.update(t); - /* check for requested subscriptions */ if (_subscribe_to_stream != nullptr) { if (OK == configure_stream(_subscribe_to_stream, _subscribe_to_stream_rate)) { if (_subscribe_to_stream_rate > 0.0f) { - warnx("stream %s on device %s enabled with rate %.1f Hz", _subscribe_to_stream, _device_name, (double)_subscribe_to_stream_rate); + warnx("stream %s on device %s enabled with rate %.1f Hz", _subscribe_to_stream, _device_name, + (double)_subscribe_to_stream_rate); } else { warnx("stream %s on device %s disabled", _subscribe_to_stream, _device_name); @@ -2057,88 +1455,68 @@ Mavlink::task_main(int argc, char *argv[]) stream->update(t); } - bool updated; - orb_check(mission_result_sub, &updated); - - if (updated) { - orb_copy(ORB_ID(mission_result), mission_result_sub, &mission_result); - - if (_verbose) { warnx("Got mission result: new current: %d", mission_result.index_current_mission); } - - if (mission_result.mission_reached) { - mavlink_wpm_send_waypoint_reached((uint16_t)mission_result.mission_index_reached); - } - - mavlink_wpm_send_waypoint_current((uint16_t)mission_result.index_current_mission); - - } else { - if (slow_rate_limiter.check(t)) { - mavlink_wpm_send_waypoint_current((uint16_t)mission_result.index_current_mission); - } - } - - if (fast_rate_limiter.check(t)) { - mavlink_pm_queued_send(); - mavlink_waypoint_eventloop(hrt_absolute_time()); - - if (!mavlink_logbuffer_is_empty(&_logbuffer)) { - struct mavlink_logmessage msg; - int lb_ret = mavlink_logbuffer_read(&_logbuffer, &msg); - - if (lb_ret == OK) { - mavlink_missionlib_send_gcs_string(msg.text); - } - } - } - /* pass messages from other UARTs or FTP worker */ if (_passing_on || _ftp_on) { bool is_part; uint8_t *read_ptr; - uint8_t *write_ptr; + uint8_t *write_ptr; pthread_mutex_lock(&_message_buffer_mutex); - int available = message_buffer_get_ptr((void**)&read_ptr, &is_part); + int available = message_buffer_get_ptr((void **)&read_ptr, &is_part); pthread_mutex_unlock(&_message_buffer_mutex); if (available > 0) { - // Reconstruct message from buffer + // Reconstruct message from buffer mavlink_message_t msg; - write_ptr = (uint8_t*)&msg; - - // Pull a single message from the buffer - size_t read_count = available; - if (read_count > sizeof(mavlink_message_t)) { - read_count = sizeof(mavlink_message_t); - } - - memcpy(write_ptr, read_ptr, read_count); - - // We hold the mutex until after we complete the second part of the buffer. If we don't - // we may end up breaking the empty slot overflow detection semantics when we mark the - // possibly partial read below. - pthread_mutex_lock(&_message_buffer_mutex); - + write_ptr = (uint8_t *)&msg; + + // Pull a single message from the buffer + size_t read_count = available; + + if (read_count > sizeof(mavlink_message_t)) { + read_count = sizeof(mavlink_message_t); + } + + memcpy(write_ptr, read_ptr, read_count); + + // We hold the mutex until after we complete the second part of the buffer. If we don't + // we may end up breaking the empty slot overflow detection semantics when we mark the + // possibly partial read below. + pthread_mutex_lock(&_message_buffer_mutex); + message_buffer_mark_read(read_count); /* write second part of buffer if there is some */ if (is_part && read_count < sizeof(mavlink_message_t)) { - write_ptr += read_count; - available = message_buffer_get_ptr((void**)&read_ptr, &is_part); - read_count = sizeof(mavlink_message_t) - read_count; - memcpy(write_ptr, read_ptr, read_count); + write_ptr += read_count; + available = message_buffer_get_ptr((void **)&read_ptr, &is_part); + read_count = sizeof(mavlink_message_t) - read_count; + memcpy(write_ptr, read_ptr, read_count); message_buffer_mark_read(available); } - - pthread_mutex_unlock(&_message_buffer_mutex); - _mavlink_resend_uart(_channel, &msg); + pthread_mutex_unlock(&_message_buffer_mutex); + + resend_message(&msg); } } + /* update TX/RX rates*/ + if (t > _bytes_timestamp + 1000000) { + if (_bytes_timestamp != 0) { + float dt = (t - _bytes_timestamp) / 1000.0f; + _rate_tx = _bytes_tx / dt; + _rate_txerr = _bytes_txerr / dt; + _rate_rx = _bytes_rx / dt; + _bytes_tx = 0; + _bytes_txerr = 0; + _bytes_rx = 0; + } + _bytes_timestamp = t; + } perf_end(_loop_perf); } @@ -2188,6 +1566,7 @@ Mavlink::task_main(int argc, char *argv[]) message_buffer_destroy(); pthread_mutex_destroy(&_message_buffer_mutex); } + /* destroy log buffer */ mavlink_logbuffer_destroy(&_logbuffer); @@ -2209,6 +1588,7 @@ int Mavlink::start_helper(int argc, char *argv[]) /* out of memory */ res = -ENOMEM; warnx("OUT OF MEM"); + } else { /* this will actually only return once MAVLink exits */ res = instance->task_main(argc, argv); @@ -2238,7 +1618,7 @@ Mavlink::start(int argc, char *argv[]) task_spawn_cmd(buf, SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, - 1950, + 2700, (main_t)&Mavlink::start_helper, (const char **)argv); @@ -2268,7 +1648,42 @@ Mavlink::start(int argc, char *argv[]) void Mavlink::display_status() { - warnx("running"); + + if (_rstatus.heartbeat_time > 0) { + printf("\tGCS heartbeat:\t%llu us ago\n", hrt_elapsed_time(&_rstatus.heartbeat_time)); + } + + if (_rstatus.timestamp > 0) { + + printf("\ttype:\t\t"); + + switch (_rstatus.type) { + case TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO: + printf("3DR RADIO\n"); + break; + + default: + printf("UNKNOWN RADIO\n"); + break; + } + + printf("\trssi:\t\t%d\n", _rstatus.rssi); + printf("\tremote rssi:\t%u\n", _rstatus.remote_rssi); + printf("\ttxbuf:\t\t%u\n", _rstatus.txbuf); + printf("\tnoise:\t\t%d\n", _rstatus.noise); + printf("\tremote noise:\t%u\n", _rstatus.remote_noise); + printf("\trx errors:\t%u\n", _rstatus.rxerrors); + printf("\tfixed:\t\t%u\n", _rstatus.fixed); + + } else { + printf("\tno telem status.\n"); + } + + printf("\trates:\n"); + printf("\ttx: %.3f kB/s\n", (double)_rate_tx); + printf("\ttxerr: %.3f kB/s\n", (double)_rate_txerr); + printf("\trx: %.3f kB/s\n", (double)_rate_rx); + printf("\trate mult: %.3f\n", (double)_rate_mult); } int @@ -2356,8 +1771,8 @@ int mavlink_main(int argc, char *argv[]) } else if (!strcmp(argv[1], "stop-all")) { return Mavlink::destroy_all_instances(); - // } else if (!strcmp(argv[1], "status")) { - // mavlink::g_mavlink->status(); + } else if (!strcmp(argv[1], "status")) { + return Mavlink::get_status_all_instances(); } else if (!strcmp(argv[1], "stream")) { return Mavlink::stream_command(argc, argv); diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index f94036a17..1e2e94cef 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -50,54 +50,15 @@ #include <uORB/uORB.h> #include <uORB/topics/mission.h> +#include <uORB/topics/mission_result.h> +#include <uORB/topics/telemetry_status.h> #include "mavlink_bridge_header.h" #include "mavlink_orb_subscription.h" #include "mavlink_stream.h" #include "mavlink_messages.h" - -// FIXME XXX - TO BE MOVED TO XML -enum MAVLINK_WPM_STATES { - MAVLINK_WPM_STATE_IDLE = 0, - MAVLINK_WPM_STATE_SENDLIST, - MAVLINK_WPM_STATE_SENDLIST_SENDWPS, - MAVLINK_WPM_STATE_GETLIST, - MAVLINK_WPM_STATE_GETLIST_GETWPS, - MAVLINK_WPM_STATE_GETLIST_GOTALL, - MAVLINK_WPM_STATE_ENUM_END -}; - -enum MAVLINK_WPM_CODES { - MAVLINK_WPM_CODE_OK = 0, - MAVLINK_WPM_CODE_ERR_WAYPOINT_ACTION_NOT_SUPPORTED, - MAVLINK_WPM_CODE_ERR_WAYPOINT_FRAME_NOT_SUPPORTED, - MAVLINK_WPM_CODE_ERR_WAYPOINT_OUT_OF_BOUNDS, - MAVLINK_WPM_CODE_ERR_WAYPOINT_MAX_NUMBER_EXCEEDED, - MAVLINK_WPM_CODE_ENUM_END -}; - - -#define MAVLINK_WPM_MAX_WP_COUNT 255 -#define MAVLINK_WPM_PROTOCOL_TIMEOUT_DEFAULT 5000000 ///< Protocol communication timeout in useconds -#define MAVLINK_WPM_SETPOINT_DELAY_DEFAULT 1000000 ///< When to send a new setpoint -#define MAVLINK_WPM_PROTOCOL_DELAY_DEFAULT 40000 - - -struct mavlink_wpm_storage { - uint16_t size; - uint16_t max_size; - enum MAVLINK_WPM_STATES current_state; - int16_t current_wp_id; ///< Waypoint in current transmission - uint16_t current_count; - uint8_t current_partner_sysid; - uint8_t current_partner_compid; - uint64_t timestamp_lastaction; - uint64_t timestamp_last_send_setpoint; - uint64_t timestamp_last_send_request; - uint32_t timeout; - int current_dataman_id; -}; - +#include "mavlink_mission.h" +#include "mavlink_parameters.h" class Mavlink { @@ -137,9 +98,11 @@ public: static int destroy_all_instances(); + static int get_status_all_instances(); + static bool instance_exists(const char *device_name, Mavlink *self); - static void forward_message(mavlink_message_t *msg, Mavlink *self); + static void forward_message(const mavlink_message_t *msg, Mavlink *self); static int get_uart_fd(unsigned index); @@ -178,11 +141,6 @@ public: bool get_forwarding_on() { return _forwarding_on; } - /** - * Handle waypoint related messages. - */ - void mavlink_wpm_message_handler(const mavlink_message_t *msg); - static int start_helper(int argc, char *argv[]); /** @@ -202,7 +160,16 @@ public: */ int set_hil_enabled(bool hil_enabled); - MavlinkOrbSubscription *add_orb_subscription(const orb_id_t topic); + void send_message(const uint8_t msgid, const void *msg); + + /** + * Resend message as is, don't change sequence number and CRC. + */ + void resend_message(mavlink_message_t *msg); + + void handle_message(const mavlink_message_t *msg); + + MavlinkOrbSubscription *add_orb_subscription(const orb_id_t topic); int get_instance_id(); @@ -215,14 +182,47 @@ public: mavlink_channel_t get_channel(); - void configure_stream_threadsafe(const char *stream_name, float rate); + void configure_stream_threadsafe(const char *stream_name, float rate); bool _task_should_exit; /**< if true, mavlink task should exit */ int get_mavlink_fd() { return _mavlink_fd; } - MavlinkStream * get_streams() const { return _streams; } + /** + * Send a status text with loglevel INFO + * + * @param string the message to send (will be capped by mavlink max string length) + */ + void send_statustext_info(const char *string); + + /** + * Send a status text with loglevel CRITICAL + * + * @param string the message to send (will be capped by mavlink max string length) + */ + void send_statustext_critical(const char *string); + + /** + * Send a status text with loglevel EMERGENCY + * + * @param string the message to send (will be capped by mavlink max string length) + */ + void send_statustext_emergency(const char *string); + /** + * Send a status text with loglevel, the difference from mavlink_log_xxx() is that message sent + * only on this mavlink connection. Useful for reporting communication specific, not system-wide info + * only to client interested in it. Message will be not sent immediately but queued in buffer as + * for mavlink_log_xxx(). + * + * @param string the message to send (will be capped by mavlink max string length) + * @param severity the log level + */ + void send_statustext(unsigned char severity, const char *string); + + MavlinkStream * get_streams() const { return _streams; } + + float get_rate_mult(); /* Functions for waiting to start transmission until message received. */ void set_has_received_messages(bool received_messages) { _received_messages = received_messages; } @@ -231,15 +231,37 @@ public: bool get_wait_to_transmit() { return _wait_to_transmit; } bool should_transmit() { return (!_wait_to_transmit || (_wait_to_transmit && _received_messages)); } - bool message_buffer_write(void *ptr, int size); + bool message_buffer_write(const void *ptr, int size); - void lockMessageBufferMutex(void) { pthread_mutex_lock(&_message_buffer_mutex); } - void unlockMessageBufferMutex(void) { pthread_mutex_unlock(&_message_buffer_mutex); } + void lockMessageBufferMutex(void) { pthread_mutex_lock(&_message_buffer_mutex); } + void unlockMessageBufferMutex(void) { pthread_mutex_unlock(&_message_buffer_mutex); } /** * Count a transmision error */ - void count_txerr(); + void count_txerr(); + + /** + * Count transmitted bytes + */ + void count_txbytes(unsigned n) { _bytes_tx += n; }; + + /** + * Count bytes not transmitted because of errors + */ + void count_txerrbytes(unsigned n) { _bytes_txerr += n; }; + + /** + * Count received bytes + */ + void count_rxbytes(unsigned n) { _bytes_rx += n; }; + + /** + * Get the receive status of this MAVLink link + */ + struct telemetry_status_s& get_rx_status() { return _rstatus; } + + struct mavlink_logbuffer *get_logbuffer() { return &_logbuffer; } protected: Mavlink *next; @@ -262,11 +284,11 @@ private: MavlinkOrbSubscription *_subscriptions; MavlinkStream *_streams; - orb_advert_t _mission_pub; - struct mission_s mission; - MAVLINK_MODE _mode; + MavlinkMissionManager *_mission_manager; + MavlinkParametersManager *_parameters_manager; + + MAVLINK_MODE _mode; - uint8_t _mavlink_wpm_comp_id; mavlink_channel_t _channel; struct mavlink_logbuffer _logbuffer; @@ -274,17 +296,15 @@ private: pthread_t _receive_thread; - /* Allocate storage space for waypoints */ - mavlink_wpm_storage _wpm_s; - mavlink_wpm_storage *_wpm; - bool _verbose; bool _forwarding_on; bool _passing_on; bool _ftp_on; int _uart_fd; int _baudrate; - int _datarate; + int _datarate; ///< data rate for normal streams (attitude, position, etc.) + int _datarate_events; ///< data rate for params, waypoints, text messages + float _rate_mult; /** * If the queue index is not at 0, the queue sending @@ -299,6 +319,18 @@ private: float _subscribe_to_stream_rate; bool _flow_control_enabled; + uint64_t _last_write_success_time; + uint64_t _last_write_try_time; + + unsigned _bytes_tx; + unsigned _bytes_txerr; + unsigned _bytes_rx; + uint64_t _bytes_timestamp; + float _rate_tx; + float _rate_txerr; + float _rate_rx; + + struct telemetry_status_s _rstatus; ///< receive status struct mavlink_message_buffer { int write_ptr; @@ -310,6 +342,7 @@ private: mavlink_message_buffer _message_buffer; pthread_mutex_t _message_buffer_mutex; + pthread_mutex_t _send_mutex; bool _param_initialized; param_t _param_system_id; @@ -320,67 +353,27 @@ private: perf_counter_t _loop_perf; /**< loop performance counter */ perf_counter_t _txerr_perf; /**< TX error counter */ - /** - * Send one parameter. - * - * @param param The parameter id to send. - * @return zero on success, nonzero on failure. - */ - int mavlink_pm_send_param(param_t param); + void mavlink_update_system(); - /** - * Send one parameter identified by index. - * - * @param index The index of the parameter to send. - * @return zero on success, nonzero else. - */ - int mavlink_pm_send_param_for_index(uint16_t index); + int mavlink_open_uart(int baudrate, const char *uart_name, struct termios *uart_config_original, bool *is_usb); /** - * Send one parameter identified by name. + * Get the free space in the transmit buffer * - * @param name The index of the parameter to send. - * @return zero on success, nonzero else. + * @return free space in the UART TX buffer */ - int mavlink_pm_send_param_for_name(const char *name); + unsigned get_free_tx_buf(); - /** - * Send a queue of parameters, one parameter per function call. - * - * @return zero on success, nonzero on failure - */ - int mavlink_pm_queued_send(void); + static unsigned int interval_from_rate(float rate); + + int configure_stream(const char *stream_name, const float rate); /** - * Start sending the parameter queue. + * Adjust the stream rates based on the current rate * - * This function will not directly send parameters, but instead - * activate the sending of one parameter on each call of - * mavlink_pm_queued_send(). - * @see mavlink_pm_queued_send() + * @param multiplier if greater than 1, the transmission rate will increase, if smaller than one decrease */ - void mavlink_pm_start_queued_send(); - - void mavlink_update_system(); - - void mavlink_waypoint_eventloop(uint64_t now); - void mavlink_wpm_send_waypoint_reached(uint16_t seq); - void mavlink_wpm_send_waypoint_request(uint8_t sysid, uint8_t compid, uint16_t seq); - void mavlink_wpm_send_waypoint(uint8_t sysid, uint8_t compid, uint16_t seq); - void mavlink_wpm_send_waypoint_count(uint8_t sysid, uint8_t compid, uint16_t count); - void mavlink_wpm_send_waypoint_current(uint16_t seq); - void mavlink_wpm_send_waypoint_ack(uint8_t sysid, uint8_t compid, uint8_t type); - void mavlink_wpm_init(mavlink_wpm_storage *state); - int map_mission_item_to_mavlink_mission_item(const struct mission_item_s *mission_item, mavlink_mission_item_t *mavlink_mission_item); - int map_mavlink_mission_item_to_mission_item(const mavlink_mission_item_t *mavlink_mission_item, struct mission_item_s *mission_item); - void publish_mission(); - - void mavlink_missionlib_send_message(mavlink_message_t *msg); - int mavlink_missionlib_send_gcs_string(const char *string); - - int mavlink_open_uart(int baudrate, const char *uart_name, struct termios *uart_config_original, bool *is_usb); - - int configure_stream(const char *stream_name, const float rate); + void adjust_stream_rates(const float multiplier); int message_buffer_init(int size); @@ -394,7 +387,12 @@ private: void message_buffer_mark_read(int n); - void pass_message(mavlink_message_t *msg); + void pass_message(const mavlink_message_t *msg); + + /** + * Update rate mult so total bitrate will be equal to _datarate. + */ + void update_rate_mult(); static int mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg); @@ -403,4 +401,7 @@ private: */ int task_main(int argc, char *argv[]); + /* do not allow copying this class */ + Mavlink(const Mavlink&); + Mavlink operator=(const Mavlink&); }; diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index e91b73d1b..c10be77b5 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -40,9 +40,9 @@ */ #include <stdio.h> + #include <commander/px4_custom_mode.h> #include <lib/geo/geo.h> - #include <uORB/uORB.h> #include <uORB/topics/sensor_combined.h> #include <uORB/topics/vehicle_attitude.h> @@ -72,11 +72,11 @@ #include <drivers/drv_rc_input.h> #include <drivers/drv_pwm_output.h> #include <drivers/drv_range_finder.h> - #include <systemlib/err.h> +#include <mavlink/mavlink_log.h> #include "mavlink_messages.h" - +#include "mavlink_main.h" static uint16_t cm_uint16_from_m_float(float m); static void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_setpoint_triplet_s *pos_sp_triplet, @@ -170,6 +170,8 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set break; case NAVIGATION_STATE_LAND: + /* fallthrough */ + case NAVIGATION_STATE_DESCEND: *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; @@ -190,6 +192,17 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL; break; + case NAVIGATION_STATE_OFFBOARD: + *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED + | MAV_MODE_FLAG_STABILIZE_ENABLED + | MAV_MODE_FLAG_GUIDED_ENABLED; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_OFFBOARD; + break; + + case NAVIGATION_STATE_MAX: + /* this is an unused case, ignore */ + break; + } *mavlink_custom_mode = custom_mode.data; @@ -236,21 +249,33 @@ public: return MAVLINK_MSG_ID_HEARTBEAT; } - static MavlinkStream *new_instance() + static MavlinkStream *new_instance(Mavlink *mavlink) { - return new MavlinkStreamHeartbeat(); + return new MavlinkStreamHeartbeat(mavlink); + } + + unsigned get_size() + { + return MAVLINK_MSG_ID_HEARTBEAT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + } + + bool const_rate() { + return true; } private: - MavlinkOrbSubscription *status_sub; - MavlinkOrbSubscription *pos_sp_triplet_sub; + MavlinkOrbSubscription *_status_sub; + MavlinkOrbSubscription *_pos_sp_triplet_sub; + + /* do not allow top copying this class */ + MavlinkStreamHeartbeat(MavlinkStreamHeartbeat &); + MavlinkStreamHeartbeat& operator = (const MavlinkStreamHeartbeat &); protected: - void subscribe(Mavlink *mavlink) - { - status_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_status)); - pos_sp_triplet_sub = mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet)); - } + explicit MavlinkStreamHeartbeat(Mavlink *mavlink) : MavlinkStream(mavlink), + _status_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_status))), + _pos_sp_triplet_sub(_mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet))) + {} void send(const hrt_abstime t) { @@ -258,30 +283,150 @@ protected: struct position_setpoint_triplet_s pos_sp_triplet; /* always send the heartbeat, independent of the update status of the topics */ - if (!status_sub->update(&status)) { + if (!_status_sub->update(&status)) { /* if topic update failed fill it with defaults */ memset(&status, 0, sizeof(status)); } - if (!pos_sp_triplet_sub->update(&pos_sp_triplet)) { + if (!_pos_sp_triplet_sub->update(&pos_sp_triplet)) { /* if topic update failed fill it with defaults */ memset(&pos_sp_triplet, 0, sizeof(pos_sp_triplet)); } - uint8_t mavlink_state = 0; - uint8_t mavlink_base_mode = 0; - uint32_t mavlink_custom_mode = 0; - get_mavlink_mode_state(&status, &pos_sp_triplet, &mavlink_state, &mavlink_base_mode, &mavlink_custom_mode); + mavlink_heartbeat_t msg; + + msg.base_mode = 0; + msg.custom_mode = 0; + get_mavlink_mode_state(&status, &pos_sp_triplet, &msg.system_status, &msg.base_mode, &msg.custom_mode); + msg.type = mavlink_system.type; + msg.autopilot = MAV_AUTOPILOT_PX4; + msg.mavlink_version = 3; + + _mavlink->send_message(MAVLINK_MSG_ID_HEARTBEAT, &msg); + } +}; + +class MavlinkStreamStatustext : public MavlinkStream +{ +public: + const char *get_name() const + { + return MavlinkStreamStatustext::get_name_static(); + } + + static const char *get_name_static() + { + return "STATUSTEXT"; + } + + uint8_t get_id() + { + return MAVLINK_MSG_ID_STATUSTEXT; + } + + static MavlinkStream *new_instance(Mavlink *mavlink) + { + return new MavlinkStreamStatustext(mavlink); + } + + unsigned get_size() { + return mavlink_logbuffer_is_empty(_mavlink->get_logbuffer()) ? 0 : (MAVLINK_MSG_ID_STATUSTEXT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES); + } + +private: + /* do not allow top copying this class */ + MavlinkStreamStatustext(MavlinkStreamStatustext &); + MavlinkStreamStatustext& operator = (const MavlinkStreamStatustext &); + +protected: + explicit MavlinkStreamStatustext(Mavlink *mavlink) : MavlinkStream(mavlink) + {} + + void send(const hrt_abstime t) + { + if (!mavlink_logbuffer_is_empty(_mavlink->get_logbuffer())) { + struct mavlink_logmessage logmsg; + int lb_ret = mavlink_logbuffer_read(_mavlink->get_logbuffer(), &logmsg); + + if (lb_ret == OK) { + mavlink_statustext_t msg; + + msg.severity = logmsg.severity; + strncpy(msg.text, logmsg.text, sizeof(msg.text)); - mavlink_msg_heartbeat_send(_channel, - mavlink_system.type, - MAV_AUTOPILOT_PX4, - mavlink_base_mode, - mavlink_custom_mode, - mavlink_state); + _mavlink->send_message(MAVLINK_MSG_ID_STATUSTEXT, &msg); + } + } } }; +class MavlinkStreamCommandLong : public MavlinkStream +{ +public: + const char *get_name() const + { + return MavlinkStreamCommandLong::get_name_static(); + } + + static const char *get_name_static() + { + return "COMMAND_LONG"; + } + + uint8_t get_id() + { + return MAVLINK_MSG_ID_COMMAND_LONG; + } + + static MavlinkStream *new_instance(Mavlink *mavlink) + { + return new MavlinkStreamCommandLong(mavlink); + } + + unsigned get_size() { + return 0; // commands stream is not regular and not predictable + } + +private: + MavlinkOrbSubscription *_cmd_sub; + uint64_t _cmd_time; + + /* do not allow top copying this class */ + MavlinkStreamCommandLong(MavlinkStreamCommandLong &); + MavlinkStreamCommandLong& operator = (const MavlinkStreamCommandLong &); + +protected: + explicit MavlinkStreamCommandLong(Mavlink *mavlink) : MavlinkStream(mavlink), + _cmd_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_command))), + _cmd_time(0) + {} + + void send(const hrt_abstime t) + { + struct vehicle_command_s cmd; + + if (_cmd_sub->update(&_cmd_time, &cmd)) { + /* only send commands for other systems/components */ + if (cmd.target_system != mavlink_system.sysid || cmd.target_component != mavlink_system.compid) { + mavlink_command_long_t msg; + + msg.target_system = cmd.target_system; + msg.target_component = cmd.target_component; + msg.command = cmd.command; + msg.confirmation = cmd.confirmation; + msg.param1 = cmd.param1; + msg.param2 = cmd.param2; + msg.param3 = cmd.param3; + msg.param4 = cmd.param4; + msg.param5 = cmd.param5; + msg.param6 = cmd.param6; + msg.param7 = cmd.param7; + + _mavlink->send_message(MAVLINK_MSG_ID_COMMAND_LONG, &msg); + } + } + } +}; class MavlinkStreamSysStatus : public MavlinkStream { @@ -291,7 +436,7 @@ public: return MavlinkStreamSysStatus::get_name_static(); } - static const char *get_name_static () + static const char *get_name_static() { return "SYS_STATUS"; } @@ -301,39 +446,50 @@ public: return MAVLINK_MSG_ID_SYS_STATUS; } - static MavlinkStream *new_instance() + static MavlinkStream *new_instance(Mavlink *mavlink) + { + return new MavlinkStreamSysStatus(mavlink); + } + + unsigned get_size() { - return new MavlinkStreamSysStatus(); + return MAVLINK_MSG_ID_SYS_STATUS_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; } private: - MavlinkOrbSubscription *status_sub; + MavlinkOrbSubscription *_status_sub; + + /* do not allow top copying this class */ + MavlinkStreamSysStatus(MavlinkStreamSysStatus &); + MavlinkStreamSysStatus& operator = (const MavlinkStreamSysStatus &); protected: - void subscribe(Mavlink *mavlink) - { - status_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_status)); - } + explicit MavlinkStreamSysStatus(Mavlink *mavlink) : MavlinkStream(mavlink), + _status_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_status))) + {} void send(const hrt_abstime t) { struct vehicle_status_s status; - if (status_sub->update(&status)) { - mavlink_msg_sys_status_send(_channel, - status.onboard_control_sensors_present, - status.onboard_control_sensors_enabled, - status.onboard_control_sensors_health, - status.load * 1000.0f, - status.battery_voltage * 1000.0f, - status.battery_current * 100.0f, - status.battery_remaining * 100.0f, - status.drop_rate_comm, - status.errors_comm, - status.errors_count1, - status.errors_count2, - status.errors_count3, - status.errors_count4); + if (_status_sub->update(&status)) { + mavlink_sys_status_t msg; + + msg.onboard_control_sensors_present = status.onboard_control_sensors_present; + msg.onboard_control_sensors_enabled = status.onboard_control_sensors_enabled; + msg.onboard_control_sensors_health = status.onboard_control_sensors_health; + msg.load = status.load * 1000.0f; + msg.voltage_battery = status.battery_voltage * 1000.0f; + msg.current_battery = status.battery_current * 100.0f; + msg.drop_rate_comm = status.drop_rate_comm; + msg.errors_comm = status.errors_comm; + msg.errors_count1 = status.errors_count1; + msg.errors_count2 = status.errors_count2; + msg.errors_count3 = status.errors_count3; + msg.errors_count4 = status.errors_count4; + msg.battery_remaining = status.battery_remaining * 100.0f; + + _mavlink->send_message(MAVLINK_MSG_ID_SYS_STATUS, &msg); } } }; @@ -357,73 +513,89 @@ public: return MAVLINK_MSG_ID_HIGHRES_IMU; } - static MavlinkStream *new_instance() + static MavlinkStream *new_instance(Mavlink *mavlink) + { + return new MavlinkStreamHighresIMU(mavlink); + } + + unsigned get_size() { - return new MavlinkStreamHighresIMU(); + return MAVLINK_MSG_ID_HIGHRES_IMU_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; } private: - MavlinkOrbSubscription *sensor_sub; - uint64_t sensor_time; + MavlinkOrbSubscription *_sensor_sub; + uint64_t _sensor_time; + + uint64_t _accel_timestamp; + uint64_t _gyro_timestamp; + uint64_t _mag_timestamp; + uint64_t _baro_timestamp; - uint64_t accel_timestamp; - uint64_t gyro_timestamp; - uint64_t mag_timestamp; - uint64_t baro_timestamp; + /* do not allow top copying this class */ + MavlinkStreamHighresIMU(MavlinkStreamHighresIMU &); + MavlinkStreamHighresIMU& operator = (const MavlinkStreamHighresIMU &); protected: - explicit MavlinkStreamHighresIMU() : MavlinkStream(), - sensor_time(0), - accel_timestamp(0), - gyro_timestamp(0), - mag_timestamp(0), - baro_timestamp(0) + explicit MavlinkStreamHighresIMU(Mavlink *mavlink) : MavlinkStream(mavlink), + _sensor_sub(_mavlink->add_orb_subscription(ORB_ID(sensor_combined))), + _sensor_time(0), + _accel_timestamp(0), + _gyro_timestamp(0), + _mag_timestamp(0), + _baro_timestamp(0) {} - void subscribe(Mavlink *mavlink) - { - sensor_sub = mavlink->add_orb_subscription(ORB_ID(sensor_combined)); - } - void send(const hrt_abstime t) { struct sensor_combined_s sensor; - if (sensor_sub->update(&sensor_time, &sensor)) { + if (_sensor_sub->update(&_sensor_time, &sensor)) { uint16_t fields_updated = 0; - if (accel_timestamp != sensor.accelerometer_timestamp) { + if (_accel_timestamp != sensor.accelerometer_timestamp) { /* mark first three dimensions as changed */ fields_updated |= (1 << 0) | (1 << 1) | (1 << 2); - accel_timestamp = sensor.accelerometer_timestamp; + _accel_timestamp = sensor.accelerometer_timestamp; } - if (gyro_timestamp != sensor.timestamp) { + if (_gyro_timestamp != sensor.timestamp) { /* mark second group dimensions as changed */ fields_updated |= (1 << 3) | (1 << 4) | (1 << 5); - gyro_timestamp = sensor.timestamp; + _gyro_timestamp = sensor.timestamp; } - if (mag_timestamp != sensor.magnetometer_timestamp) { + if (_mag_timestamp != sensor.magnetometer_timestamp) { /* mark third group dimensions as changed */ fields_updated |= (1 << 6) | (1 << 7) | (1 << 8); - mag_timestamp = sensor.magnetometer_timestamp; + _mag_timestamp = sensor.magnetometer_timestamp; } - if (baro_timestamp != sensor.baro_timestamp) { + if (_baro_timestamp != sensor.baro_timestamp) { /* mark last group dimensions as changed */ fields_updated |= (1 << 9) | (1 << 11) | (1 << 12); - baro_timestamp = sensor.baro_timestamp; + _baro_timestamp = sensor.baro_timestamp; } - mavlink_msg_highres_imu_send(_channel, - sensor.timestamp, - sensor.accelerometer_m_s2[0], sensor.accelerometer_m_s2[1], sensor.accelerometer_m_s2[2], - sensor.gyro_rad_s[0], sensor.gyro_rad_s[1], sensor.gyro_rad_s[2], - sensor.magnetometer_ga[0], sensor.magnetometer_ga[1], sensor.magnetometer_ga[2], - sensor.baro_pres_mbar, sensor.differential_pressure_pa, - sensor.baro_alt_meter, sensor.baro_temp_celcius, - fields_updated); + mavlink_highres_imu_t msg; + + msg.time_usec = sensor.timestamp; + msg.xacc = sensor.accelerometer_m_s2[0]; + msg.yacc = sensor.accelerometer_m_s2[1]; + msg.zacc = sensor.accelerometer_m_s2[2]; + msg.xgyro = sensor.gyro_rad_s[0]; + msg.ygyro = sensor.gyro_rad_s[1]; + msg.zgyro = sensor.gyro_rad_s[2]; + msg.xmag = sensor.magnetometer_ga[0]; + msg.ymag = sensor.magnetometer_ga[1]; + msg.zmag = sensor.magnetometer_ga[2]; + msg.abs_pressure = sensor.baro_pres_mbar; + msg.diff_pressure = sensor.differential_pressure_pa; + msg.pressure_alt = sensor.baro_alt_meter; + msg.temperature = sensor.baro_temp_celcius; + msg.fields_updated = fields_updated; + + _mavlink->send_message(MAVLINK_MSG_ID_HIGHRES_IMU, &msg); } } }; @@ -447,34 +619,47 @@ public: return MAVLINK_MSG_ID_ATTITUDE; } - static MavlinkStream *new_instance() + static MavlinkStream *new_instance(Mavlink *mavlink) { - return new MavlinkStreamAttitude(); + return new MavlinkStreamAttitude(mavlink); + } + + unsigned get_size() + { + return MAVLINK_MSG_ID_ATTITUDE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; } private: - MavlinkOrbSubscription *att_sub; - uint64_t att_time; + MavlinkOrbSubscription *_att_sub; + uint64_t _att_time; + + /* do not allow top copying this class */ + MavlinkStreamAttitude(MavlinkStreamAttitude &); + MavlinkStreamAttitude& operator = (const MavlinkStreamAttitude &); + protected: - explicit MavlinkStreamAttitude() : MavlinkStream(), - att_time(0) + explicit MavlinkStreamAttitude(Mavlink *mavlink) : MavlinkStream(mavlink), + _att_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_attitude))), + _att_time(0) {} - void subscribe(Mavlink *mavlink) - { - att_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_attitude)); - } - void send(const hrt_abstime t) { struct vehicle_attitude_s att; - if (att_sub->update(&att_time, &att)) { - mavlink_msg_attitude_send(_channel, - att.timestamp / 1000, - att.roll, att.pitch, att.yaw, - att.rollspeed, att.pitchspeed, att.yawspeed); + if (_att_sub->update(&_att_time, &att)) { + mavlink_attitude_t msg; + + msg.time_boot_ms = att.timestamp / 1000; + msg.roll = att.roll; + msg.pitch = att.pitch; + msg.yaw = att.yaw; + msg.rollspeed = att.rollspeed; + msg.pitchspeed = att.pitchspeed; + msg.yawspeed = att.yawspeed; + + _mavlink->send_message(MAVLINK_MSG_ID_ATTITUDE, &msg); } } }; @@ -498,39 +683,47 @@ public: return MAVLINK_MSG_ID_ATTITUDE_QUATERNION; } - static MavlinkStream *new_instance() + static MavlinkStream *new_instance(Mavlink *mavlink) { - return new MavlinkStreamAttitudeQuaternion(); + return new MavlinkStreamAttitudeQuaternion(mavlink); + } + + unsigned get_size() + { + return MAVLINK_MSG_ID_ATTITUDE_QUATERNION_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; } private: - MavlinkOrbSubscription *att_sub; - uint64_t att_time; + MavlinkOrbSubscription *_att_sub; + uint64_t _att_time; + + /* do not allow top copying this class */ + MavlinkStreamAttitudeQuaternion(MavlinkStreamAttitudeQuaternion &); + MavlinkStreamAttitudeQuaternion& operator = (const MavlinkStreamAttitudeQuaternion &); protected: - explicit MavlinkStreamAttitudeQuaternion() : MavlinkStream(), - att_time(0) + explicit MavlinkStreamAttitudeQuaternion(Mavlink *mavlink) : MavlinkStream(mavlink), + _att_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_attitude))), + _att_time(0) {} - void subscribe(Mavlink *mavlink) - { - att_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_attitude)); - } - void send(const hrt_abstime t) { struct vehicle_attitude_s att; - if (att_sub->update(&att_time, &att)) { - mavlink_msg_attitude_quaternion_send(_channel, - att.timestamp / 1000, - att.q[0], - att.q[1], - att.q[2], - att.q[3], - att.rollspeed, - att.pitchspeed, - att.yawspeed); + if (_att_sub->update(&_att_time, &att)) { + mavlink_attitude_quaternion_t msg; + + msg.time_boot_ms = att.timestamp / 1000; + msg.q1 = att.q[0]; + msg.q2 = att.q[1]; + msg.q3 = att.q[2]; + msg.q4 = att.q[3]; + msg.rollspeed = att.rollspeed; + msg.pitchspeed = att.pitchspeed; + msg.yawspeed = att.yawspeed; + + _mavlink->send_message(MAVLINK_MSG_ID_ATTITUDE_QUATERNION, &msg); } } }; @@ -555,45 +748,50 @@ public: return MAVLINK_MSG_ID_VFR_HUD; } - static MavlinkStream *new_instance() + static MavlinkStream *new_instance(Mavlink *mavlink) { - return new MavlinkStreamVFRHUD(); + return new MavlinkStreamVFRHUD(mavlink); + } + + unsigned get_size() + { + return MAVLINK_MSG_ID_VFR_HUD_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; } private: - MavlinkOrbSubscription *att_sub; - uint64_t att_time; + MavlinkOrbSubscription *_att_sub; + uint64_t _att_time; - MavlinkOrbSubscription *pos_sub; - uint64_t pos_time; + MavlinkOrbSubscription *_pos_sub; + uint64_t _pos_time; - MavlinkOrbSubscription *armed_sub; - uint64_t armed_time; + MavlinkOrbSubscription *_armed_sub; + uint64_t _armed_time; - MavlinkOrbSubscription *act_sub; - uint64_t act_time; + MavlinkOrbSubscription *_act_sub; + uint64_t _act_time; - MavlinkOrbSubscription *airspeed_sub; - uint64_t airspeed_time; + MavlinkOrbSubscription *_airspeed_sub; + uint64_t _airspeed_time; + + /* do not allow top copying this class */ + MavlinkStreamVFRHUD(MavlinkStreamVFRHUD &); + MavlinkStreamVFRHUD& operator = (const MavlinkStreamVFRHUD &); protected: - explicit MavlinkStreamVFRHUD() : MavlinkStream(), - att_time(0), - pos_time(0), - armed_time(0), - act_time(0), - airspeed_time(0) + explicit MavlinkStreamVFRHUD(Mavlink *mavlink) : MavlinkStream(mavlink), + _att_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_attitude))), + _att_time(0), + _pos_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_global_position))), + _pos_time(0), + _armed_sub(_mavlink->add_orb_subscription(ORB_ID(actuator_armed))), + _armed_time(0), + _act_sub(_mavlink->add_orb_subscription(ORB_ID(actuator_controls_0))), + _act_time(0), + _airspeed_sub(_mavlink->add_orb_subscription(ORB_ID(airspeed))), + _airspeed_time(0) {} - void subscribe(Mavlink *mavlink) - { - att_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_attitude)); - pos_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_global_position)); - armed_sub = mavlink->add_orb_subscription(ORB_ID(actuator_armed)); - act_sub = mavlink->add_orb_subscription(ORB_ID(actuator_controls_0)); - airspeed_sub = mavlink->add_orb_subscription(ORB_ID(airspeed)); - } - void send(const hrt_abstime t) { struct vehicle_attitude_s att; @@ -602,24 +800,23 @@ protected: struct actuator_controls_s act; struct airspeed_s airspeed; - bool updated = att_sub->update(&att_time, &att); - updated |= pos_sub->update(&pos_time, &pos); - updated |= armed_sub->update(&armed_time, &armed); - updated |= act_sub->update(&act_time, &act); - updated |= airspeed_sub->update(&airspeed_time, &airspeed); + bool updated = _att_sub->update(&_att_time, &att); + updated |= _pos_sub->update(&_pos_time, &pos); + updated |= _armed_sub->update(&_armed_time, &armed); + updated |= _act_sub->update(&_act_time, &act); + updated |= _airspeed_sub->update(&_airspeed_time, &airspeed); if (updated) { - float groundspeed = sqrtf(pos.vel_n * pos.vel_n + pos.vel_e * pos.vel_e); - uint16_t heading = _wrap_2pi(att.yaw) * M_RAD_TO_DEG_F; - float throttle = armed.armed ? act.control[3] * 100.0f : 0.0f; - - mavlink_msg_vfr_hud_send(_channel, - airspeed.true_airspeed_m_s, - groundspeed, - heading, - throttle, - pos.alt, - -pos.vel_d); + mavlink_vfr_hud_t msg; + + msg.airspeed = airspeed.true_airspeed_m_s; + msg.groundspeed = sqrtf(pos.vel_n * pos.vel_n + pos.vel_e * pos.vel_e); + msg.heading = _wrap_2pi(att.yaw) * M_RAD_TO_DEG_F; + msg.throttle = armed.armed ? act.control[3] * 100.0f : 0.0f; + msg.alt = pos.alt; + msg.climb = -pos.vel_d; + + _mavlink->send_message(MAVLINK_MSG_ID_VFR_HUD, &msg); } } }; @@ -643,41 +840,49 @@ public: return MAVLINK_MSG_ID_GPS_RAW_INT; } - static MavlinkStream *new_instance() + static MavlinkStream *new_instance(Mavlink *mavlink) + { + return new MavlinkStreamGPSRawInt(mavlink); + } + + unsigned get_size() { - return new MavlinkStreamGPSRawInt(); + return MAVLINK_MSG_ID_GPS_RAW_INT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; } private: - MavlinkOrbSubscription *gps_sub; - uint64_t gps_time; + MavlinkOrbSubscription *_gps_sub; + uint64_t _gps_time; + + /* do not allow top copying this class */ + MavlinkStreamGPSRawInt(MavlinkStreamGPSRawInt &); + MavlinkStreamGPSRawInt& operator = (const MavlinkStreamGPSRawInt &); protected: - explicit MavlinkStreamGPSRawInt() : MavlinkStream(), - gps_time(0) + explicit MavlinkStreamGPSRawInt(Mavlink *mavlink) : MavlinkStream(mavlink), + _gps_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_gps_position))), + _gps_time(0) {} - void subscribe(Mavlink *mavlink) - { - gps_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_gps_position)); - } - void send(const hrt_abstime t) { struct vehicle_gps_position_s gps; - if (gps_sub->update(&gps_time, &gps)) { - mavlink_msg_gps_raw_int_send(_channel, - gps.timestamp_position, - gps.fix_type, - gps.lat, - gps.lon, - gps.alt, - cm_uint16_from_m_float(gps.eph), - cm_uint16_from_m_float(gps.epv), - gps.vel_m_s * 100.0f, - _wrap_2pi(gps.cog_rad) * M_RAD_TO_DEG_F * 1e2f, - gps.satellites_visible); + if (_gps_sub->update(&_gps_time, &gps)) { + mavlink_gps_raw_int_t msg; + + msg.time_usec = gps.timestamp_position; + msg.fix_type = gps.fix_type; + msg.lat = gps.lat; + msg.lon = gps.lon; + msg.alt = gps.alt; + msg.eph = cm_uint16_from_m_float(gps.eph); + msg.epv = cm_uint16_from_m_float(gps.epv); + msg.vel = cm_uint16_from_m_float(gps.vel_m_s), + msg.cog = _wrap_2pi(gps.cog_rad) * M_RAD_TO_DEG_F * 1e2f, + msg.satellites_visible = gps.satellites_used; + + _mavlink->send_message(MAVLINK_MSG_ID_GPS_RAW_INT, &msg); } } }; @@ -701,49 +906,57 @@ public: return MAVLINK_MSG_ID_GLOBAL_POSITION_INT; } - static MavlinkStream *new_instance() + static MavlinkStream *new_instance(Mavlink *mavlink) { - return new MavlinkStreamGlobalPositionInt(); + return new MavlinkStreamGlobalPositionInt(mavlink); + } + + unsigned get_size() + { + return MAVLINK_MSG_ID_GLOBAL_POSITION_INT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; } private: - MavlinkOrbSubscription *pos_sub; - uint64_t pos_time; + MavlinkOrbSubscription *_pos_sub; + uint64_t _pos_time; + + MavlinkOrbSubscription *_home_sub; + uint64_t _home_time; - MavlinkOrbSubscription *home_sub; - uint64_t home_time; + /* do not allow top copying this class */ + MavlinkStreamGlobalPositionInt(MavlinkStreamGlobalPositionInt &); + MavlinkStreamGlobalPositionInt& operator = (const MavlinkStreamGlobalPositionInt &); protected: - explicit MavlinkStreamGlobalPositionInt() : MavlinkStream(), - pos_time(0), - home_time(0) + explicit MavlinkStreamGlobalPositionInt(Mavlink *mavlink) : MavlinkStream(mavlink), + _pos_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_global_position))), + _pos_time(0), + _home_sub(_mavlink->add_orb_subscription(ORB_ID(home_position))), + _home_time(0) {} - void subscribe(Mavlink *mavlink) - { - pos_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_global_position)); - home_sub = mavlink->add_orb_subscription(ORB_ID(home_position)); - } - void send(const hrt_abstime t) { struct vehicle_global_position_s pos; struct home_position_s home; - bool updated = pos_sub->update(&pos_time, &pos); - updated |= home_sub->update(&home_time, &home); + bool updated = _pos_sub->update(&_pos_time, &pos); + updated |= _home_sub->update(&_home_time, &home); if (updated) { - mavlink_msg_global_position_int_send(_channel, - pos.timestamp / 1000, - pos.lat * 1e7, - pos.lon * 1e7, - pos.alt * 1000.0f, - (pos.alt - home.alt) * 1000.0f, - pos.vel_n * 100.0f, - pos.vel_e * 100.0f, - pos.vel_d * 100.0f, - _wrap_2pi(pos.yaw) * M_RAD_TO_DEG_F * 100.0f); + mavlink_global_position_int_t msg; + + msg.time_boot_ms = pos.timestamp / 1000; + msg.lat = pos.lat * 1e7; + msg.lon = pos.lon * 1e7; + msg.alt = pos.alt * 1000.0f; + msg.relative_alt = (pos.alt - home.alt) * 1000.0f; + msg.vx = pos.vel_n * 100.0f; + msg.vy = pos.vel_e * 100.0f; + msg.vz = pos.vel_d * 100.0f; + msg.hdg = _wrap_2pi(pos.yaw) * M_RAD_TO_DEG_F * 100.0f; + + _mavlink->send_message(MAVLINK_MSG_ID_GLOBAL_POSITION_INT, &msg); } } }; @@ -767,44 +980,51 @@ public: return MAVLINK_MSG_ID_LOCAL_POSITION_NED; } - static MavlinkStream *new_instance() + static MavlinkStream *new_instance(Mavlink *mavlink) + { + return new MavlinkStreamLocalPositionNED(mavlink); + } + + unsigned get_size() { - return new MavlinkStreamLocalPositionNED(); + return MAVLINK_MSG_ID_LOCAL_POSITION_NED_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; } private: - MavlinkOrbSubscription *pos_sub; - uint64_t pos_time; + MavlinkOrbSubscription *_pos_sub; + uint64_t _pos_time; + + /* do not allow top copying this class */ + MavlinkStreamLocalPositionNED(MavlinkStreamLocalPositionNED &); + MavlinkStreamLocalPositionNED& operator = (const MavlinkStreamLocalPositionNED &); protected: - explicit MavlinkStreamLocalPositionNED() : MavlinkStream(), - pos_time(0) + explicit MavlinkStreamLocalPositionNED(Mavlink *mavlink) : MavlinkStream(mavlink), + _pos_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_local_position))), + _pos_time(0) {} - void subscribe(Mavlink *mavlink) - { - pos_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_local_position)); - } - void send(const hrt_abstime t) { struct vehicle_local_position_s pos; - if (pos_sub->update(&pos_time, &pos)) { - mavlink_msg_local_position_ned_send(_channel, - pos.timestamp / 1000, - pos.x, - pos.y, - pos.z, - pos.vx, - pos.vy, - pos.vz); + if (_pos_sub->update(&_pos_time, &pos)) { + mavlink_local_position_ned_t msg; + + msg.time_boot_ms = pos.timestamp / 1000; + msg.x = pos.x; + msg.y = pos.y; + msg.z = pos.z; + msg.vx = pos.vx; + msg.vy = pos.vy; + msg.vz = pos.vz; + + _mavlink->send_message(MAVLINK_MSG_ID_LOCAL_POSITION_NED, &msg); } } }; - class MavlinkStreamViconPositionEstimate : public MavlinkStream { public: @@ -823,38 +1043,46 @@ public: return MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE; } - static MavlinkStream *new_instance() + static MavlinkStream *new_instance(Mavlink *mavlink) + { + return new MavlinkStreamViconPositionEstimate(mavlink); + } + + unsigned get_size() { - return new MavlinkStreamViconPositionEstimate(); + return MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; } private: - MavlinkOrbSubscription *pos_sub; - uint64_t pos_time; + MavlinkOrbSubscription *_pos_sub; + uint64_t _pos_time; + + /* do not allow top copying this class */ + MavlinkStreamViconPositionEstimate(MavlinkStreamViconPositionEstimate &); + MavlinkStreamViconPositionEstimate& operator = (const MavlinkStreamViconPositionEstimate &); protected: - explicit MavlinkStreamViconPositionEstimate() : MavlinkStream(), - pos_time(0) + explicit MavlinkStreamViconPositionEstimate(Mavlink *mavlink) : MavlinkStream(mavlink), + _pos_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_vicon_position))), + _pos_time(0) {} - void subscribe(Mavlink *mavlink) - { - pos_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_vicon_position)); - } - void send(const hrt_abstime t) { struct vehicle_vicon_position_s pos; - if (pos_sub->update(&pos_time, &pos)) { - mavlink_msg_vicon_position_estimate_send(_channel, - pos.timestamp / 1000, - pos.x, - pos.y, - pos.z, - pos.roll, - pos.pitch, - pos.yaw); + if (_pos_sub->update(&_pos_time, &pos)) { + mavlink_vicon_position_estimate_t msg; + + msg.usec = pos.timestamp; + msg.x = pos.x; + msg.y = pos.y; + msg.z = pos.z; + msg.roll = pos.roll; + msg.pitch = pos.pitch; + msg.yaw = pos.yaw; + + _mavlink->send_message(MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE, &msg); } } }; @@ -878,37 +1106,49 @@ public: return MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN; } - static MavlinkStream *new_instance() + static MavlinkStream *new_instance(Mavlink *mavlink) + { + return new MavlinkStreamGPSGlobalOrigin(mavlink); + } + + unsigned get_size() { - return new MavlinkStreamGPSGlobalOrigin(); + return _home_sub->is_published() ? (MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0; } private: - MavlinkOrbSubscription *home_sub; + MavlinkOrbSubscription *_home_sub; + + /* do not allow top copying this class */ + MavlinkStreamGPSGlobalOrigin(MavlinkStreamGPSGlobalOrigin &); + MavlinkStreamGPSGlobalOrigin& operator = (const MavlinkStreamGPSGlobalOrigin &); protected: - void subscribe(Mavlink *mavlink) - { - home_sub = mavlink->add_orb_subscription(ORB_ID(home_position)); - } + explicit MavlinkStreamGPSGlobalOrigin(Mavlink *mavlink) : MavlinkStream(mavlink), + _home_sub(_mavlink->add_orb_subscription(ORB_ID(home_position))) + {} void send(const hrt_abstime t) { /* we're sending the GPS home periodically to ensure the * the GCS does pick it up at one point */ - if (home_sub->is_published()) { + if (_home_sub->is_published()) { struct home_position_s home; - if (home_sub->update(&home)) { - mavlink_msg_gps_global_origin_send(_channel, - (int32_t)(home.lat * 1e7), - (int32_t)(home.lon * 1e7), - (int32_t)(home.alt) * 1000.0f); + if (_home_sub->update(&home)) { + mavlink_gps_global_origin_t msg; + + msg.latitude = home.lat * 1e7; + msg.longitude = home.lon * 1e7; + msg.altitude = home.alt * 1e3f; + + _mavlink->send_message(MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN, &msg); } } } }; + template <int N> class MavlinkStreamServoOutputRaw : public MavlinkStream { @@ -940,21 +1180,28 @@ public: } } - static MavlinkStream *new_instance() + static MavlinkStream *new_instance(Mavlink *mavlink) + { + return new MavlinkStreamServoOutputRaw<N>(mavlink); + } + + unsigned get_size() { - return new MavlinkStreamServoOutputRaw<N>(); + return MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; } private: - MavlinkOrbSubscription *act_sub; - uint64_t act_time; + MavlinkOrbSubscription *_act_sub; + uint64_t _act_time; -protected: - explicit MavlinkStreamServoOutputRaw() : MavlinkStream(), - act_time(0) - {} + /* do not allow top copying this class */ + MavlinkStreamServoOutputRaw(MavlinkStreamServoOutputRaw &); + MavlinkStreamServoOutputRaw& operator = (const MavlinkStreamServoOutputRaw &); - void subscribe(Mavlink *mavlink) +protected: + explicit MavlinkStreamServoOutputRaw(Mavlink *mavlink) : MavlinkStream(mavlink), + _act_sub(nullptr), + _act_time(0) { orb_id_t act_topics[] = { ORB_ID(actuator_outputs_0), @@ -963,25 +1210,28 @@ protected: ORB_ID(actuator_outputs_3) }; - act_sub = mavlink->add_orb_subscription(act_topics[N]); + _act_sub = _mavlink->add_orb_subscription(act_topics[N]); } void send(const hrt_abstime t) { struct actuator_outputs_s act; - if (act_sub->update(&act_time, &act)) { - mavlink_msg_servo_output_raw_send(_channel, - act.timestamp / 1000, - N, - act.output[0], - act.output[1], - act.output[2], - act.output[3], - act.output[4], - act.output[5], - act.output[6], - act.output[7]); + if (_act_sub->update(&_act_time, &act)) { + mavlink_servo_output_raw_t msg; + + msg.time_usec = act.timestamp; + msg.port = N; + msg.servo1_raw = act.output[0]; + msg.servo2_raw = act.output[1]; + msg.servo3_raw = act.output[2]; + msg.servo4_raw = act.output[3]; + msg.servo5_raw = act.output[4]; + msg.servo6_raw = act.output[5]; + msg.servo7_raw = act.output[6]; + msg.servo8_raw = act.output[7]; + + _mavlink->send_message(MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, &msg); } } }; @@ -1005,44 +1255,49 @@ public: return MAVLINK_MSG_ID_HIL_CONTROLS; } - static MavlinkStream *new_instance() + static MavlinkStream *new_instance(Mavlink *mavlink) + { + return new MavlinkStreamHILControls(mavlink); + } + + unsigned get_size() { - return new MavlinkStreamHILControls(); + return MAVLINK_MSG_ID_HIL_CONTROLS_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; } private: - MavlinkOrbSubscription *status_sub; - uint64_t status_time; + MavlinkOrbSubscription *_status_sub; + uint64_t _status_time; + + MavlinkOrbSubscription *_pos_sp_triplet_sub; + uint64_t _pos_sp_triplet_time; - MavlinkOrbSubscription *pos_sp_triplet_sub; - uint64_t pos_sp_triplet_time; + MavlinkOrbSubscription *_act_sub; + uint64_t _act_time; - MavlinkOrbSubscription *act_sub; - uint64_t act_time; + /* do not allow top copying this class */ + MavlinkStreamHILControls(MavlinkStreamHILControls &); + MavlinkStreamHILControls& operator = (const MavlinkStreamHILControls &); protected: - explicit MavlinkStreamHILControls() : MavlinkStream(), - status_time(0), - pos_sp_triplet_time(0), - act_time(0) + explicit MavlinkStreamHILControls(Mavlink *mavlink) : MavlinkStream(mavlink), + _status_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_status))), + _status_time(0), + _pos_sp_triplet_sub(_mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet))), + _pos_sp_triplet_time(0), + _act_sub(_mavlink->add_orb_subscription(ORB_ID(actuator_outputs_0))), + _act_time(0) {} - void subscribe(Mavlink *mavlink) - { - status_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_status)); - pos_sp_triplet_sub = mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet)); - act_sub = mavlink->add_orb_subscription(ORB_ID(actuator_outputs_0)); - } - void send(const hrt_abstime t) { struct vehicle_status_s status; struct position_setpoint_triplet_s pos_sp_triplet; struct actuator_outputs_s act; - bool updated = act_sub->update(&act_time, &act); - updated |= pos_sp_triplet_sub->update(&pos_sp_triplet_time, &pos_sp_triplet); - updated |= status_sub->update(&status_time, &status); + bool updated = _act_sub->update(&_act_time, &act); + updated |= _pos_sp_triplet_sub->update(&_pos_sp_triplet_time, &pos_sp_triplet); + updated |= _status_sub->update(&_status_time, &status); if (updated && (status.arming_state == ARMING_STATE_ARMED)) { /* translate the current syste state to mavlink state and mode */ @@ -1051,10 +1306,16 @@ protected: uint32_t mavlink_custom_mode; get_mavlink_mode_state(&status, &pos_sp_triplet, &mavlink_state, &mavlink_base_mode, &mavlink_custom_mode); + float out[8]; + + const float pwm_center = (PWM_HIGHEST_MAX + PWM_LOWEST_MIN) / 2; + + /* scale outputs depending on system type */ if (mavlink_system.type == MAV_TYPE_QUADROTOR || mavlink_system.type == MAV_TYPE_HEXAROTOR || mavlink_system.type == MAV_TYPE_OCTOROTOR) { - /* set number of valid outputs depending on vehicle type */ + /* multirotors: set number of rotor outputs depending on type */ + unsigned n; switch (mavlink_system.type) { @@ -1071,107 +1332,112 @@ protected: break; } - /* scale / assign outputs depending on system type */ - float out[8]; - for (unsigned i = 0; i < 8; i++) { - if (i < n) { - if (mavlink_base_mode & MAV_MODE_FLAG_SAFETY_ARMED) { - /* scale fake PWM out 900..2100 us to 0..1 for normal multirotors */ + if (mavlink_base_mode & MAV_MODE_FLAG_SAFETY_ARMED) { + if (i < n) { + /* scale PWM out 900..2100 us to 0..1 for rotors */ out[i] = (act.output[i] - PWM_LOWEST_MIN) / (PWM_HIGHEST_MAX - PWM_LOWEST_MIN); } else { - /* send 0 when disarmed */ - out[i] = 0.0f; + /* scale PWM out 900..2100 us to -1..1 for other channels */ + out[i] = (act.output[i] - pwm_center) / ((PWM_HIGHEST_MAX - PWM_LOWEST_MIN) / 2); } } else { - out[i] = -1.0f; + /* send 0 when disarmed */ + out[i] = 0.0f; } } - mavlink_msg_hil_controls_send(_channel, - hrt_absolute_time(), - out[0], out[1], out[2], out[3], out[4], out[5], out[6], out[7], - mavlink_base_mode, - 0); } else { - - /* fixed wing: scale all channels except throttle -1 .. 1 - * because we know that we set the mixers up this way - */ - - float out[8]; - - const float pwm_center = (PWM_HIGHEST_MAX + PWM_LOWEST_MIN) / 2; + /* fixed wing: scale throttle to 0..1 and other channels to -1..1 */ for (unsigned i = 0; i < 8; i++) { if (i != 3) { - /* scale fake PWM out 900..2100 us to -1..+1 for normal channels */ + /* scale PWM out 900..2100 us to -1..1 for normal channels */ out[i] = (act.output[i] - pwm_center) / ((PWM_HIGHEST_MAX - PWM_LOWEST_MIN) / 2); } else { - - /* scale fake PWM out 900..2100 us to 0..1 for throttle */ + /* scale PWM out 900..2100 us to 0..1 for throttle */ out[i] = (act.output[i] - PWM_LOWEST_MIN) / (PWM_HIGHEST_MAX - PWM_LOWEST_MIN); } } - - mavlink_msg_hil_controls_send(_channel, - hrt_absolute_time(), - out[0], out[1], out[2], out[3], out[4], out[5], out[6], out[7], - mavlink_base_mode, - 0); } + + mavlink_hil_controls_t msg; + + msg.time_usec = hrt_absolute_time(); + msg.roll_ailerons = out[0]; + msg.pitch_elevator = out[1]; + msg.yaw_rudder = out[2]; + msg.throttle = out[3]; + msg.aux1 = out[4]; + msg.aux2 = out[5]; + msg.aux3 = out[6]; + msg.aux4 = out[7]; + msg.mode = mavlink_base_mode; + msg.nav_mode = 0; + + _mavlink->send_message(MAVLINK_MSG_ID_HIL_CONTROLS, &msg); } } }; -class MavlinkStreamGlobalPositionSetpointInt : public MavlinkStream +class MavlinkStreamPositionTargetGlobalInt : public MavlinkStream { public: const char *get_name() const { - return MavlinkStreamGlobalPositionSetpointInt::get_name_static(); + return MavlinkStreamPositionTargetGlobalInt::get_name_static(); } static const char *get_name_static() { - return "GLOBAL_POSITION_SETPOINT_INT"; + return "POSITION_TARGET_GLOBAL_INT"; } uint8_t get_id() { - return MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT; + return MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT; } - static MavlinkStream *new_instance() + static MavlinkStream *new_instance(Mavlink *mavlink) { - return new MavlinkStreamGlobalPositionSetpointInt(); + return new MavlinkStreamPositionTargetGlobalInt(mavlink); + } + + unsigned get_size() + { + return MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; } private: - MavlinkOrbSubscription *pos_sp_triplet_sub; + MavlinkOrbSubscription *_pos_sp_triplet_sub; + + /* do not allow top copying this class */ + MavlinkStreamPositionTargetGlobalInt(MavlinkStreamPositionTargetGlobalInt &); + MavlinkStreamPositionTargetGlobalInt& operator = (const MavlinkStreamPositionTargetGlobalInt &); protected: - void subscribe(Mavlink *mavlink) - { - pos_sp_triplet_sub = mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet)); - } + explicit MavlinkStreamPositionTargetGlobalInt(Mavlink *mavlink) : MavlinkStream(mavlink), + _pos_sp_triplet_sub(_mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet))) + {} void send(const hrt_abstime t) { struct position_setpoint_triplet_s pos_sp_triplet; - if (pos_sp_triplet_sub->update(&pos_sp_triplet)) { - mavlink_msg_global_position_setpoint_int_send(_channel, - MAV_FRAME_GLOBAL, - (int32_t)(pos_sp_triplet.current.lat * 1e7), - (int32_t)(pos_sp_triplet.current.lon * 1e7), - (int32_t)(pos_sp_triplet.current.alt * 1000), - (int16_t)(pos_sp_triplet.current.yaw * M_RAD_TO_DEG_F * 100.0f)); + if (_pos_sp_triplet_sub->update(&pos_sp_triplet)) { + mavlink_position_target_global_int_t msg{}; + + msg.coordinate_frame = MAV_FRAME_GLOBAL; + msg.lat_int = pos_sp_triplet.current.lat * 1e7; + msg.lon_int = pos_sp_triplet.current.lon * 1e7; + msg.alt = pos_sp_triplet.current.alt; + + _mavlink->send_message(MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT, &msg); } } }; @@ -1187,150 +1453,124 @@ public: static const char *get_name_static() { - return "LOCAL_POSITION_SETPOINT"; + return "POSITION_TARGET_LOCAL_NED"; } uint8_t get_id() { - return MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT; + return MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED; + } + + static MavlinkStream *new_instance(Mavlink *mavlink) + { + return new MavlinkStreamLocalPositionSetpoint(mavlink); } - static MavlinkStream *new_instance() + unsigned get_size() { - return new MavlinkStreamLocalPositionSetpoint(); + return MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; } private: - MavlinkOrbSubscription *pos_sp_sub; - uint64_t pos_sp_time; + MavlinkOrbSubscription *_pos_sp_sub; + uint64_t _pos_sp_time; + + /* do not allow top copying this class */ + MavlinkStreamLocalPositionSetpoint(MavlinkStreamLocalPositionSetpoint &); + MavlinkStreamLocalPositionSetpoint& operator = (const MavlinkStreamLocalPositionSetpoint &); protected: - explicit MavlinkStreamLocalPositionSetpoint() : MavlinkStream(), - pos_sp_time(0) + explicit MavlinkStreamLocalPositionSetpoint(Mavlink *mavlink) : MavlinkStream(mavlink), + _pos_sp_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_local_position_setpoint))), + _pos_sp_time(0) {} - void subscribe(Mavlink *mavlink) - { - pos_sp_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_local_position_setpoint)); - } - void send(const hrt_abstime t) { struct vehicle_local_position_setpoint_s pos_sp; - if (pos_sp_sub->update(&pos_sp_time, &pos_sp)) { - mavlink_msg_local_position_setpoint_send(_channel, - MAV_FRAME_LOCAL_NED, - pos_sp.x, - pos_sp.y, - pos_sp.z, - pos_sp.yaw); + if (_pos_sp_sub->update(&_pos_sp_time, &pos_sp)) { + mavlink_position_target_local_ned_t msg{}; + + msg.time_boot_ms = pos_sp.timestamp / 1000; + msg.coordinate_frame = MAV_FRAME_LOCAL_NED; + msg.x = pos_sp.x; + msg.y = pos_sp.y; + msg.z = pos_sp.z; + + _mavlink->send_message(MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED, &msg); } } }; -class MavlinkStreamRollPitchYawThrustSetpoint : public MavlinkStream +class MavlinkStreamAttitudeTarget : public MavlinkStream { public: const char *get_name() const { - return MavlinkStreamRollPitchYawThrustSetpoint::get_name_static(); + return MavlinkStreamAttitudeTarget::get_name_static(); } static const char *get_name_static() { - return "ROLL_PITCH_YAW_THRUST_SETPOINT"; + return "ATTITUDE_TARGET"; } uint8_t get_id() { - return MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT; + return MAVLINK_MSG_ID_ATTITUDE_TARGET; + } + + static MavlinkStream *new_instance(Mavlink *mavlink) + { + return new MavlinkStreamAttitudeTarget(mavlink); } - static MavlinkStream *new_instance() + unsigned get_size() { - return new MavlinkStreamRollPitchYawThrustSetpoint(); + return MAVLINK_MSG_ID_ATTITUDE_TARGET_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; } private: - MavlinkOrbSubscription *att_sp_sub; - uint64_t att_sp_time; + MavlinkOrbSubscription *_att_sp_sub; + MavlinkOrbSubscription *_att_rates_sp_sub; + uint64_t _att_sp_time; + uint64_t _att_rates_sp_time; + + /* do not allow top copying this class */ + MavlinkStreamAttitudeTarget(MavlinkStreamAttitudeTarget &); + MavlinkStreamAttitudeTarget& operator = (const MavlinkStreamAttitudeTarget &); protected: - explicit MavlinkStreamRollPitchYawThrustSetpoint() : MavlinkStream(), - att_sp_time(0) + explicit MavlinkStreamAttitudeTarget(Mavlink *mavlink) : MavlinkStream(mavlink), + _att_sp_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_attitude_setpoint))), + _att_rates_sp_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_rates_setpoint))), + _att_sp_time(0), + _att_rates_sp_time(0) {} - void subscribe(Mavlink *mavlink) - { - att_sp_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_attitude_setpoint)); - } - void send(const hrt_abstime t) { struct vehicle_attitude_setpoint_s att_sp; - if (att_sp_sub->update(&att_sp_time, &att_sp)) { - mavlink_msg_roll_pitch_yaw_thrust_setpoint_send(_channel, - att_sp.timestamp / 1000, - att_sp.roll_body, - att_sp.pitch_body, - att_sp.yaw_body, - att_sp.thrust); - } - } -}; + if (_att_sp_sub->update(&_att_sp_time, &att_sp)) { + struct vehicle_rates_setpoint_s att_rates_sp; + (void)_att_rates_sp_sub->update(&_att_rates_sp_time, &att_rates_sp); -class MavlinkStreamRollPitchYawRatesThrustSetpoint : public MavlinkStream -{ -public: - const char *get_name() const - { - return MavlinkStreamRollPitchYawRatesThrustSetpoint::get_name_static(); - } - - static const char *get_name_static() - { - return "ROLL_PITCH_YAW_RATES_THRUST_SETPOINT"; - } + mavlink_attitude_target_t msg{}; - uint8_t get_id() - { - return MAVLINK_MSG_ID_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT; - } + msg.time_boot_ms = att_sp.timestamp / 1000; + mavlink_euler_to_quaternion(att_sp.roll_body, att_sp.pitch_body, att_sp.yaw_body, msg.q); - static MavlinkStream *new_instance() - { - return new MavlinkStreamRollPitchYawRatesThrustSetpoint(); - } + msg.body_roll_rate = att_rates_sp.roll; + msg.body_pitch_rate = att_rates_sp.pitch; + msg.body_yaw_rate = att_rates_sp.yaw; -private: - MavlinkOrbSubscription *att_rates_sp_sub; - uint64_t att_rates_sp_time; + msg.thrust = att_sp.thrust; -protected: - explicit MavlinkStreamRollPitchYawRatesThrustSetpoint() : MavlinkStream(), - att_rates_sp_time(0) - {} - - void subscribe(Mavlink *mavlink) - { - att_rates_sp_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_rates_setpoint)); - } - - void send(const hrt_abstime t) - { - struct vehicle_rates_setpoint_s att_rates_sp; - - if (att_rates_sp_sub->update(&att_rates_sp_time, &att_rates_sp)) { - mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_send(_channel, - att_rates_sp.timestamp / 1000, - att_rates_sp.roll, - att_rates_sp.pitch, - att_rates_sp.yaw, - att_rates_sp.thrust); + _mavlink->send_message(MAVLINK_MSG_ID_ATTITUDE_TARGET, &msg); } } }; @@ -1354,72 +1594,84 @@ public: return MAVLINK_MSG_ID_RC_CHANNELS_RAW; } - static MavlinkStream *new_instance() + static MavlinkStream *new_instance(Mavlink *mavlink) + { + return new MavlinkStreamRCChannelsRaw(mavlink); + } + + unsigned get_size() { - return new MavlinkStreamRCChannelsRaw(); + return _rc_sub->is_published() ? ((RC_INPUT_MAX_CHANNELS / 8) * (MAVLINK_MSG_ID_RC_CHANNELS_RAW_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) + + MAVLINK_MSG_ID_RC_CHANNELS_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0; } private: - MavlinkOrbSubscription *rc_sub; - uint64_t rc_time; + MavlinkOrbSubscription *_rc_sub; + uint64_t _rc_time; + + /* do not allow top copying this class */ + MavlinkStreamRCChannelsRaw(MavlinkStreamRCChannelsRaw &); + MavlinkStreamRCChannelsRaw& operator = (const MavlinkStreamRCChannelsRaw &); protected: - explicit MavlinkStreamRCChannelsRaw() : MavlinkStream(), - rc_time(0) + explicit MavlinkStreamRCChannelsRaw(Mavlink *mavlink) : MavlinkStream(mavlink), + _rc_sub(_mavlink->add_orb_subscription(ORB_ID(input_rc))), + _rc_time(0) {} - void subscribe(Mavlink *mavlink) - { - rc_sub = mavlink->add_orb_subscription(ORB_ID(input_rc)); - } - void send(const hrt_abstime t) { struct rc_input_values rc; - if (rc_sub->update(&rc_time, &rc)) { + if (_rc_sub->update(&_rc_time, &rc)) { const unsigned port_width = 8; - // Deprecated message (but still needed for compatibility!) + /* deprecated message (but still needed for compatibility!) */ for (unsigned i = 0; (i * port_width) < rc.channel_count; i++) { - /* Channels are sent in MAVLink main loop at a fixed interval */ - mavlink_msg_rc_channels_raw_send(_channel, - rc.timestamp_publication / 1000, - i, - (rc.channel_count > (i * port_width) + 0) ? rc.values[(i * port_width) + 0] : UINT16_MAX, - (rc.channel_count > (i * port_width) + 1) ? rc.values[(i * port_width) + 1] : UINT16_MAX, - (rc.channel_count > (i * port_width) + 2) ? rc.values[(i * port_width) + 2] : UINT16_MAX, - (rc.channel_count > (i * port_width) + 3) ? rc.values[(i * port_width) + 3] : UINT16_MAX, - (rc.channel_count > (i * port_width) + 4) ? rc.values[(i * port_width) + 4] : UINT16_MAX, - (rc.channel_count > (i * port_width) + 5) ? rc.values[(i * port_width) + 5] : UINT16_MAX, - (rc.channel_count > (i * port_width) + 6) ? rc.values[(i * port_width) + 6] : UINT16_MAX, - (rc.channel_count > (i * port_width) + 7) ? rc.values[(i * port_width) + 7] : UINT16_MAX, - rc.rssi); + /* channels are sent in MAVLink main loop at a fixed interval */ + mavlink_rc_channels_raw_t msg; + + msg.time_boot_ms = rc.timestamp_publication / 1000; + msg.port = i; + msg.chan1_raw = (rc.channel_count > (i * port_width) + 0) ? rc.values[(i * port_width) + 0] : UINT16_MAX; + msg.chan2_raw = (rc.channel_count > (i * port_width) + 1) ? rc.values[(i * port_width) + 1] : UINT16_MAX; + msg.chan3_raw = (rc.channel_count > (i * port_width) + 2) ? rc.values[(i * port_width) + 2] : UINT16_MAX; + msg.chan4_raw = (rc.channel_count > (i * port_width) + 3) ? rc.values[(i * port_width) + 3] : UINT16_MAX; + msg.chan4_raw = (rc.channel_count > (i * port_width) + 4) ? rc.values[(i * port_width) + 4] : UINT16_MAX; + msg.chan4_raw = (rc.channel_count > (i * port_width) + 5) ? rc.values[(i * port_width) + 5] : UINT16_MAX; + msg.chan4_raw = (rc.channel_count > (i * port_width) + 6) ? rc.values[(i * port_width) + 6] : UINT16_MAX; + msg.chan4_raw = (rc.channel_count > (i * port_width) + 7) ? rc.values[(i * port_width) + 7] : UINT16_MAX; + msg.rssi = rc.rssi; + + _mavlink->send_message(MAVLINK_MSG_ID_RC_CHANNELS_RAW, &msg); } - // New message - mavlink_msg_rc_channels_send(_channel, - rc.timestamp_publication / 1000, - rc.channel_count, - ((rc.channel_count > 0) ? rc.values[0] : UINT16_MAX), - ((rc.channel_count > 1) ? rc.values[1] : UINT16_MAX), - ((rc.channel_count > 2) ? rc.values[2] : UINT16_MAX), - ((rc.channel_count > 3) ? rc.values[3] : UINT16_MAX), - ((rc.channel_count > 4) ? rc.values[4] : UINT16_MAX), - ((rc.channel_count > 5) ? rc.values[5] : UINT16_MAX), - ((rc.channel_count > 6) ? rc.values[6] : UINT16_MAX), - ((rc.channel_count > 7) ? rc.values[7] : UINT16_MAX), - ((rc.channel_count > 8) ? rc.values[8] : UINT16_MAX), - ((rc.channel_count > 9) ? rc.values[9] : UINT16_MAX), - ((rc.channel_count > 10) ? rc.values[10] : UINT16_MAX), - ((rc.channel_count > 11) ? rc.values[11] : UINT16_MAX), - ((rc.channel_count > 12) ? rc.values[12] : UINT16_MAX), - ((rc.channel_count > 13) ? rc.values[13] : UINT16_MAX), - ((rc.channel_count > 14) ? rc.values[14] : UINT16_MAX), - ((rc.channel_count > 15) ? rc.values[15] : UINT16_MAX), - ((rc.channel_count > 16) ? rc.values[16] : UINT16_MAX), - ((rc.channel_count > 17) ? rc.values[17] : UINT16_MAX), - rc.rssi); + /* new message */ + mavlink_rc_channels_t msg; + + msg.time_boot_ms = rc.timestamp_publication / 1000; + msg.chancount = rc.channel_count; + msg.chan1_raw = (rc.channel_count > 0) ? rc.values[0] : UINT16_MAX; + msg.chan2_raw = (rc.channel_count > 1) ? rc.values[1] : UINT16_MAX; + msg.chan3_raw = (rc.channel_count > 2) ? rc.values[2] : UINT16_MAX; + msg.chan4_raw = (rc.channel_count > 3) ? rc.values[3] : UINT16_MAX; + msg.chan5_raw = (rc.channel_count > 4) ? rc.values[4] : UINT16_MAX; + msg.chan6_raw = (rc.channel_count > 5) ? rc.values[5] : UINT16_MAX; + msg.chan7_raw = (rc.channel_count > 6) ? rc.values[6] : UINT16_MAX; + msg.chan8_raw = (rc.channel_count > 7) ? rc.values[7] : UINT16_MAX; + msg.chan9_raw = (rc.channel_count > 8) ? rc.values[8] : UINT16_MAX; + msg.chan10_raw = (rc.channel_count > 9) ? rc.values[9] : UINT16_MAX; + msg.chan11_raw = (rc.channel_count > 10) ? rc.values[10] : UINT16_MAX; + msg.chan12_raw = (rc.channel_count > 11) ? rc.values[11] : UINT16_MAX; + msg.chan13_raw = (rc.channel_count > 12) ? rc.values[12] : UINT16_MAX; + msg.chan14_raw = (rc.channel_count > 13) ? rc.values[13] : UINT16_MAX; + msg.chan15_raw = (rc.channel_count > 14) ? rc.values[14] : UINT16_MAX; + msg.chan16_raw = (rc.channel_count > 15) ? rc.values[15] : UINT16_MAX; + msg.chan17_raw = (rc.channel_count > 16) ? rc.values[16] : UINT16_MAX; + msg.chan18_raw = (rc.channel_count > 17) ? rc.values[17] : UINT16_MAX; + msg.rssi = rc.rssi; + + _mavlink->send_message(MAVLINK_MSG_ID_RC_CHANNELS, &msg); } } }; @@ -1443,37 +1695,45 @@ public: return MAVLINK_MSG_ID_MANUAL_CONTROL; } - static MavlinkStream *new_instance() + static MavlinkStream *new_instance(Mavlink *mavlink) + { + return new MavlinkStreamManualControl(mavlink); + } + + unsigned get_size() { - return new MavlinkStreamManualControl(); + return _manual_sub->is_published() ? (MAVLINK_MSG_ID_MANUAL_CONTROL_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0; } private: - MavlinkOrbSubscription *manual_sub; - uint64_t manual_time; + MavlinkOrbSubscription *_manual_sub; + uint64_t _manual_time; + + /* do not allow top copying this class */ + MavlinkStreamManualControl(MavlinkStreamManualControl &); + MavlinkStreamManualControl& operator = (const MavlinkStreamManualControl &); protected: - explicit MavlinkStreamManualControl() : MavlinkStream(), - manual_time(0) + explicit MavlinkStreamManualControl(Mavlink *mavlink) : MavlinkStream(mavlink), + _manual_sub(_mavlink->add_orb_subscription(ORB_ID(manual_control_setpoint))), + _manual_time(0) {} - void subscribe(Mavlink *mavlink) - { - manual_sub = mavlink->add_orb_subscription(ORB_ID(manual_control_setpoint)); - } - void send(const hrt_abstime t) { struct manual_control_setpoint_s manual; - if (manual_sub->update(&manual_time, &manual)) { - mavlink_msg_manual_control_send(_channel, - mavlink_system.sysid, - manual.x * 1000, - manual.y * 1000, - manual.z * 1000, - manual.r * 1000, - 0); + if (_manual_sub->update(&_manual_time, &manual)) { + mavlink_manual_control_t msg; + + msg.target = mavlink_system.sysid; + msg.x = manual.x * 1000; + msg.y = manual.y * 1000; + msg.z = manual.z * 1000; + msg.r = manual.r * 1000; + msg.buttons = 0; + + _mavlink->send_message(MAVLINK_MSG_ID_MANUAL_CONTROL, &msg); } } }; @@ -1497,37 +1757,47 @@ public: return MAVLINK_MSG_ID_OPTICAL_FLOW; } - static MavlinkStream *new_instance() + static MavlinkStream *new_instance(Mavlink *mavlink) + { + return new MavlinkStreamOpticalFlow(mavlink); + } + + unsigned get_size() { - return new MavlinkStreamOpticalFlow(); + return _flow_sub->is_published() ? (MAVLINK_MSG_ID_OPTICAL_FLOW_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0; } private: - MavlinkOrbSubscription *flow_sub; - uint64_t flow_time; + MavlinkOrbSubscription *_flow_sub; + uint64_t _flow_time; + + /* do not allow top copying this class */ + MavlinkStreamOpticalFlow(MavlinkStreamOpticalFlow &); + MavlinkStreamOpticalFlow& operator = (const MavlinkStreamOpticalFlow &); protected: - explicit MavlinkStreamOpticalFlow() : MavlinkStream(), - flow_time(0) + explicit MavlinkStreamOpticalFlow(Mavlink *mavlink) : MavlinkStream(mavlink), + _flow_sub(_mavlink->add_orb_subscription(ORB_ID(optical_flow))), + _flow_time(0) {} - void subscribe(Mavlink *mavlink) - { - flow_sub = mavlink->add_orb_subscription(ORB_ID(optical_flow)); - } - void send(const hrt_abstime t) { struct optical_flow_s flow; - if (flow_sub->update(&flow_time, &flow)) { - mavlink_msg_optical_flow_send(_channel, - flow.timestamp, - flow.sensor_id, - flow.flow_raw_x, flow.flow_raw_y, - flow.flow_comp_x_m, flow.flow_comp_y_m, - flow.quality, - flow.ground_distance_m); + if (_flow_sub->update(&_flow_time, &flow)) { + mavlink_optical_flow_t msg; + + msg.time_usec = flow.timestamp; + msg.sensor_id = flow.sensor_id; + msg.flow_x = flow.flow_raw_x; + msg.flow_y = flow.flow_raw_y; + msg.flow_comp_m_x = flow.flow_comp_x_m; + msg.flow_comp_m_y = flow.flow_comp_y_m; + msg.quality = flow.quality; + msg.ground_distance = flow.ground_distance_m; + + _mavlink->send_message(MAVLINK_MSG_ID_OPTICAL_FLOW, &msg); } } }; @@ -1550,51 +1820,64 @@ public: return 0; } - static MavlinkStream *new_instance() + static MavlinkStream *new_instance(Mavlink *mavlink) + { + return new MavlinkStreamAttitudeControls(mavlink); + } + + unsigned get_size() { - return new MavlinkStreamAttitudeControls(); + return 4 * (MAVLINK_MSG_ID_NAMED_VALUE_FLOAT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES); } private: - MavlinkOrbSubscription *att_ctrl_sub; - uint64_t att_ctrl_time; + MavlinkOrbSubscription *_att_ctrl_sub; + uint64_t _att_ctrl_time; + + /* do not allow top copying this class */ + MavlinkStreamAttitudeControls(MavlinkStreamAttitudeControls &); + MavlinkStreamAttitudeControls& operator = (const MavlinkStreamAttitudeControls &); protected: - explicit MavlinkStreamAttitudeControls() : MavlinkStream(), - att_ctrl_time(0) + explicit MavlinkStreamAttitudeControls(Mavlink *mavlink) : MavlinkStream(mavlink), + _att_ctrl_sub(_mavlink->add_orb_subscription(ORB_ID_VEHICLE_ATTITUDE_CONTROLS)), + _att_ctrl_time(0) {} - void subscribe(Mavlink *mavlink) - { - att_ctrl_sub = mavlink->add_orb_subscription(ORB_ID_VEHICLE_ATTITUDE_CONTROLS); - } - void send(const hrt_abstime t) { struct actuator_controls_s att_ctrl; - if (att_ctrl_sub->update(&att_ctrl_time, &att_ctrl)) { + if (_att_ctrl_sub->update(&_att_ctrl_time, &att_ctrl)) { /* send, add spaces so that string buffer is at least 10 chars long */ - mavlink_msg_named_value_float_send(_channel, - att_ctrl.timestamp / 1000, - "rll ctrl ", - att_ctrl.control[0]); - mavlink_msg_named_value_float_send(_channel, - att_ctrl.timestamp / 1000, - "ptch ctrl ", - att_ctrl.control[1]); - mavlink_msg_named_value_float_send(_channel, - att_ctrl.timestamp / 1000, - "yaw ctrl ", - att_ctrl.control[2]); - mavlink_msg_named_value_float_send(_channel, - att_ctrl.timestamp / 1000, - "thr ctrl ", - att_ctrl.control[3]); + mavlink_named_value_float_t msg; + + msg.time_boot_ms = att_ctrl.timestamp / 1000; + + snprintf(msg.name, sizeof(msg.name), "rll ctrl"); + msg.value = att_ctrl.control[0]; + + _mavlink->send_message(MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, &msg); + + snprintf(msg.name, sizeof(msg.name), "ptch ctrl"); + msg.value = att_ctrl.control[1]; + + _mavlink->send_message(MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, &msg); + + snprintf(msg.name, sizeof(msg.name), "yaw ctrl"); + msg.value = att_ctrl.control[2]; + + _mavlink->send_message(MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, &msg); + + snprintf(msg.name, sizeof(msg.name), "thr ctrl"); + msg.value = att_ctrl.control[3]; + + _mavlink->send_message(MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, &msg); } } }; + class MavlinkStreamNamedValueFloat : public MavlinkStream { public: @@ -1613,41 +1896,49 @@ public: return MAVLINK_MSG_ID_NAMED_VALUE_FLOAT; } - static MavlinkStream *new_instance() + static MavlinkStream *new_instance(Mavlink *mavlink) + { + return new MavlinkStreamNamedValueFloat(mavlink); + } + + unsigned get_size() { - return new MavlinkStreamNamedValueFloat(); + return MAVLINK_MSG_ID_NAMED_VALUE_FLOAT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; } private: - MavlinkOrbSubscription *debug_sub; - uint64_t debug_time; + MavlinkOrbSubscription *_debug_sub; + uint64_t _debug_time; + + /* do not allow top copying this class */ + MavlinkStreamNamedValueFloat(MavlinkStreamNamedValueFloat &); + MavlinkStreamNamedValueFloat& operator = (const MavlinkStreamNamedValueFloat &); protected: - explicit MavlinkStreamNamedValueFloat() : MavlinkStream(), - debug_time(0) + explicit MavlinkStreamNamedValueFloat(Mavlink *mavlink) : MavlinkStream(mavlink), + _debug_sub(_mavlink->add_orb_subscription(ORB_ID(debug_key_value))), + _debug_time(0) {} - void subscribe(Mavlink *mavlink) - { - debug_sub = mavlink->add_orb_subscription(ORB_ID(debug_key_value)); - } - void send(const hrt_abstime t) { struct debug_key_value_s debug; - if (debug_sub->update(&debug_time, &debug)) { + if (_debug_sub->update(&_debug_time, &debug)) { + mavlink_named_value_float_t msg; + + msg.time_boot_ms = debug.timestamp_ms; + memcpy(msg.name, debug.key, sizeof(msg.name)); /* enforce null termination */ - debug.key[sizeof(debug.key) - 1] = '\0'; + msg.name[sizeof(msg.name) - 1] = '\0'; + msg.value = debug.value; - mavlink_msg_named_value_float_send(_channel, - debug.timestamp_ms, - debug.key, - debug.value); + _mavlink->send_message(MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, &msg); } } }; + class MavlinkStreamCameraCapture : public MavlinkStream { public: @@ -1666,38 +1957,53 @@ public: return 0; } - static MavlinkStream *new_instance() + static MavlinkStream *new_instance(Mavlink *mavlink) { - return new MavlinkStreamCameraCapture(); + return new MavlinkStreamCameraCapture(mavlink); + } + + unsigned get_size() + { + return MAVLINK_MSG_ID_COMMAND_LONG_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; } private: - MavlinkOrbSubscription *status_sub; + MavlinkOrbSubscription *_status_sub; + + /* do not allow top copying this class */ + MavlinkStreamCameraCapture(MavlinkStreamCameraCapture &); + MavlinkStreamCameraCapture& operator = (const MavlinkStreamCameraCapture &); protected: - void subscribe(Mavlink *mavlink) - { - status_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_status)); - } + explicit MavlinkStreamCameraCapture(Mavlink *mavlink) : MavlinkStream(mavlink), + _status_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_status))) + {} void send(const hrt_abstime t) { struct vehicle_status_s status; - (void)status_sub->update(&status); + (void)_status_sub->update(&status); - if (status.arming_state == ARMING_STATE_ARMED - || status.arming_state == ARMING_STATE_ARMED_ERROR) { + mavlink_command_long_t msg; - /* send camera capture on */ - mavlink_msg_command_long_send(_channel, mavlink_system.sysid, 0, MAV_CMD_DO_CONTROL_VIDEO, 0, 0, 0, 0, 1, 0, 0, 0); + msg.target_system = mavlink_system.sysid; + msg.target_component = MAV_COMP_ID_ALL; + msg.command = MAV_CMD_DO_CONTROL_VIDEO; + msg.confirmation = 0; + msg.param1 = 0; + msg.param2 = 0; + msg.param3 = 0; + /* set camera capture ON/OFF depending on arming state */ + msg.param4 = (status.arming_state == ARMING_STATE_ARMED || status.arming_state == ARMING_STATE_ARMED_ERROR) ? 1 : 0; + msg.param5 = 0; + msg.param6 = 0; + msg.param7 = 0; - } else { - /* send camera capture off */ - mavlink_msg_command_long_send(_channel, mavlink_system.sysid, 0, MAV_CMD_DO_CONTROL_VIDEO, 0, 0, 0, 0, 0, 0, 0, 0); - } + _mavlink->send_message(MAVLINK_MSG_ID_COMMAND_LONG, &msg); } }; + class MavlinkStreamDistanceSensor : public MavlinkStream { public: @@ -1716,45 +2022,58 @@ public: return MAVLINK_MSG_ID_DISTANCE_SENSOR; } - static MavlinkStream *new_instance() + static MavlinkStream *new_instance(Mavlink *mavlink) + { + return new MavlinkStreamDistanceSensor(mavlink); + } + + unsigned get_size() { - return new MavlinkStreamDistanceSensor(); + return _range_sub->is_published() ? (MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0; } private: - MavlinkOrbSubscription *range_sub; - uint64_t range_time; + MavlinkOrbSubscription *_range_sub; + uint64_t _range_time; + + /* do not allow top copying this class */ + MavlinkStreamDistanceSensor(MavlinkStreamDistanceSensor &); + MavlinkStreamDistanceSensor& operator = (const MavlinkStreamDistanceSensor &); protected: - explicit MavlinkStreamDistanceSensor() : MavlinkStream(), - range_time(0) + explicit MavlinkStreamDistanceSensor(Mavlink *mavlink) : MavlinkStream(mavlink), + _range_sub(_mavlink->add_orb_subscription(ORB_ID(sensor_range_finder))), + _range_time(0) {} - void subscribe(Mavlink *mavlink) - { - range_sub = mavlink->add_orb_subscription(ORB_ID(sensor_range_finder)); - } - void send(const hrt_abstime t) { struct range_finder_report range; - if (range_sub->update(&range_time, &range)) { + if (_range_sub->update(&_range_time, &range)) { + + mavlink_distance_sensor_t msg; - uint8_t type; + msg.time_boot_ms = range.timestamp / 1000; switch (range.type) { - case RANGE_FINDER_TYPE_LASER: - type = MAV_DISTANCE_SENSOR_LASER; + case RANGE_FINDER_TYPE_LASER: + msg.type = MAV_DISTANCE_SENSOR_LASER; + break; + + default: + msg.type = MAV_DISTANCE_SENSOR_LASER; break; } - uint8_t id = 0; - uint8_t orientation = 0; - uint8_t covariance = 20; + msg.id = 0; + msg.orientation = 0; + msg.min_distance = range.minimum_distance * 100; + msg.max_distance = range.minimum_distance * 100; + msg.current_distance = range.distance * 100; + msg.covariance = 20; - mavlink_msg_distance_sensor_send(_channel, range.timestamp / 1000, type, id, orientation, - range.minimum_distance*100, range.maximum_distance*100, range.distance*100, covariance); + _mavlink->send_message(MAVLINK_MSG_ID_DISTANCE_SENSOR, &msg); } } }; @@ -1762,6 +2081,8 @@ protected: StreamListItem *streams_list[] = { new StreamListItem(&MavlinkStreamHeartbeat::new_instance, &MavlinkStreamHeartbeat::get_name_static), + new StreamListItem(&MavlinkStreamStatustext::new_instance, &MavlinkStreamStatustext::get_name_static), + new StreamListItem(&MavlinkStreamCommandLong::new_instance, &MavlinkStreamCommandLong::get_name_static), new StreamListItem(&MavlinkStreamSysStatus::new_instance, &MavlinkStreamSysStatus::get_name_static), new StreamListItem(&MavlinkStreamHighresIMU::new_instance, &MavlinkStreamHighresIMU::get_name_static), new StreamListItem(&MavlinkStreamAttitude::new_instance, &MavlinkStreamAttitude::get_name_static), @@ -1770,16 +2091,16 @@ StreamListItem *streams_list[] = { new StreamListItem(&MavlinkStreamGPSRawInt::new_instance, &MavlinkStreamGPSRawInt::get_name_static), new StreamListItem(&MavlinkStreamGlobalPositionInt::new_instance, &MavlinkStreamGlobalPositionInt::get_name_static), new StreamListItem(&MavlinkStreamLocalPositionNED::new_instance, &MavlinkStreamLocalPositionNED::get_name_static), + new StreamListItem(&MavlinkStreamViconPositionEstimate::new_instance, &MavlinkStreamViconPositionEstimate::get_name_static), new StreamListItem(&MavlinkStreamGPSGlobalOrigin::new_instance, &MavlinkStreamGPSGlobalOrigin::get_name_static), new StreamListItem(&MavlinkStreamServoOutputRaw<0>::new_instance, &MavlinkStreamServoOutputRaw<0>::get_name_static), new StreamListItem(&MavlinkStreamServoOutputRaw<1>::new_instance, &MavlinkStreamServoOutputRaw<1>::get_name_static), new StreamListItem(&MavlinkStreamServoOutputRaw<2>::new_instance, &MavlinkStreamServoOutputRaw<2>::get_name_static), new StreamListItem(&MavlinkStreamServoOutputRaw<3>::new_instance, &MavlinkStreamServoOutputRaw<3>::get_name_static), new StreamListItem(&MavlinkStreamHILControls::new_instance, &MavlinkStreamHILControls::get_name_static), - new StreamListItem(&MavlinkStreamGlobalPositionSetpointInt::new_instance, &MavlinkStreamGlobalPositionSetpointInt::get_name_static), + new StreamListItem(&MavlinkStreamPositionTargetGlobalInt::new_instance, &MavlinkStreamPositionTargetGlobalInt::get_name_static), new StreamListItem(&MavlinkStreamLocalPositionSetpoint::new_instance, &MavlinkStreamLocalPositionSetpoint::get_name_static), - new StreamListItem(&MavlinkStreamRollPitchYawThrustSetpoint::new_instance, &MavlinkStreamRollPitchYawThrustSetpoint::get_name_static), - new StreamListItem(&MavlinkStreamRollPitchYawRatesThrustSetpoint::new_instance, &MavlinkStreamRollPitchYawRatesThrustSetpoint::get_name_static), + new StreamListItem(&MavlinkStreamAttitudeTarget::new_instance, &MavlinkStreamAttitudeTarget::get_name_static), new StreamListItem(&MavlinkStreamRCChannelsRaw::new_instance, &MavlinkStreamRCChannelsRaw::get_name_static), new StreamListItem(&MavlinkStreamManualControl::new_instance, &MavlinkStreamManualControl::get_name_static), new StreamListItem(&MavlinkStreamOpticalFlow::new_instance, &MavlinkStreamOpticalFlow::get_name_static), @@ -1787,6 +2108,5 @@ StreamListItem *streams_list[] = { new StreamListItem(&MavlinkStreamNamedValueFloat::new_instance, &MavlinkStreamNamedValueFloat::get_name_static), new StreamListItem(&MavlinkStreamCameraCapture::new_instance, &MavlinkStreamCameraCapture::get_name_static), new StreamListItem(&MavlinkStreamDistanceSensor::new_instance, &MavlinkStreamDistanceSensor::get_name_static), - new StreamListItem(&MavlinkStreamViconPositionEstimate::new_instance, &MavlinkStreamViconPositionEstimate::get_name_static), nullptr }; diff --git a/src/modules/mavlink/mavlink_messages.h b/src/modules/mavlink/mavlink_messages.h index ee64d0e42..7e4416609 100644 --- a/src/modules/mavlink/mavlink_messages.h +++ b/src/modules/mavlink/mavlink_messages.h @@ -46,10 +46,10 @@ class StreamListItem { public: - MavlinkStream* (*new_instance)(); + MavlinkStream* (*new_instance)(Mavlink *mavlink); const char* (*get_name)(); - StreamListItem(MavlinkStream* (*inst)(), const char* (*name)()) : + StreamListItem(MavlinkStream* (*inst)(Mavlink *mavlink), const char* (*name)()) : new_instance(inst), get_name(name) {}; diff --git a/src/modules/mavlink/mavlink_mission.cpp b/src/modules/mavlink/mavlink_mission.cpp new file mode 100644 index 000000000..7a761588a --- /dev/null +++ b/src/modules/mavlink/mavlink_mission.cpp @@ -0,0 +1,831 @@ +/**************************************************************************** + * + * 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 mavlink_mission.cpp + * MAVLink mission manager implementation. + * + * @author Lorenz Meier <lm@inf.ethz.ch> + * @author Julian Oes <joes@student.ethz.ch> + * @author Anton Babushkin <anton.babushkin@me.com> + */ + +#include "mavlink_mission.h" +#include "mavlink_main.h" + +#include <math.h> +#include <lib/geo/geo.h> +#include <systemlib/err.h> +#include <drivers/drv_hrt.h> + +#include <dataman/dataman.h> +#include <uORB/topics/mission.h> +#include <uORB/topics/mission_result.h> + +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +static const int ERROR = -1; + + +MavlinkMissionManager::MavlinkMissionManager(Mavlink *mavlink) : MavlinkStream(mavlink), + _state(MAVLINK_WPM_STATE_IDLE), + _time_last_recv(0), + _time_last_sent(0), + _action_timeout(MAVLINK_MISSION_PROTOCOL_TIMEOUT_DEFAULT), + _retry_timeout(MAVLINK_MISSION_RETRY_TIMEOUT_DEFAULT), + _max_count(DM_KEY_WAYPOINTS_OFFBOARD_0_MAX), + _dataman_id(0), + _count(0), + _current_seq(0), + _transfer_dataman_id(0), + _transfer_count(0), + _transfer_seq(0), + _transfer_current_seq(0), + _transfer_partner_sysid(0), + _transfer_partner_compid(0), + _offboard_mission_sub(-1), + _mission_result_sub(-1), + _offboard_mission_pub(-1), + _slow_rate_limiter(_interval / 10.0f), + _verbose(false), + _comp_id(MAV_COMP_ID_MISSIONPLANNER) +{ + _offboard_mission_sub = orb_subscribe(ORB_ID(offboard_mission)); + _mission_result_sub = orb_subscribe(ORB_ID(mission_result)); + + init_offboard_mission(); +} + +MavlinkMissionManager::~MavlinkMissionManager() +{ + close(_offboard_mission_pub); + close(_mission_result_sub); +} + +unsigned +MavlinkMissionManager::get_size() +{ + if (_state == MAVLINK_WPM_STATE_SENDLIST) { + return MAVLINK_MSG_ID_MISSION_ITEM_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + + } else if (_state == MAVLINK_WPM_STATE_GETLIST) { + return MAVLINK_MSG_ID_MISSION_REQUEST + MAVLINK_NUM_NON_PAYLOAD_BYTES; + + } else { + return 0; + } +} + +void +MavlinkMissionManager::init_offboard_mission() +{ + mission_s mission_state; + if (dm_read(DM_KEY_MISSION_STATE, 0, &mission_state, sizeof(mission_s)) == sizeof(mission_s)) { + _dataman_id = mission_state.dataman_id; + _count = mission_state.count; + _current_seq = mission_state.current_seq; + + warnx("offboard mission init: dataman_id=%d, count=%u, current_seq=%d", _dataman_id, _count, _current_seq); + + } else { + _dataman_id = 0; + _count = 0; + _current_seq = 0; + warnx("offboard mission init: ERROR, reading mission state failed"); + } +} + +/** + * Write new mission state to dataman and publish offboard_mission topic to notify navigator about changes. + */ +int +MavlinkMissionManager::update_active_mission(int dataman_id, unsigned count, int seq) +{ + struct mission_s mission; + + mission.dataman_id = dataman_id; + mission.count = count; + mission.current_seq = seq; + + /* update mission state in dataman */ + int res = dm_write(DM_KEY_MISSION_STATE, 0, DM_PERSIST_POWER_ON_RESET, &mission, sizeof(mission_s)); + + if (res == sizeof(mission_s)) { + /* update active mission state */ + _dataman_id = dataman_id; + _count = count; + _current_seq = seq; + + /* mission state saved successfully, publish offboard_mission topic */ + if (_offboard_mission_pub < 0) { + _offboard_mission_pub = orb_advertise(ORB_ID(offboard_mission), &mission); + + } else { + orb_publish(ORB_ID(offboard_mission), _offboard_mission_pub, &mission); + } + + return OK; + + } else { + warnx("ERROR: can't save mission state"); + _mavlink->send_statustext(MAV_SEVERITY_CRITICAL, "ERROR: can't save mission state"); + + return ERROR; + } +} + +void +MavlinkMissionManager::send_mission_ack(uint8_t sysid, uint8_t compid, uint8_t type) +{ + mavlink_mission_ack_t wpa; + + wpa.target_system = sysid; + wpa.target_component = compid; + wpa.type = type; + + _mavlink->send_message(MAVLINK_MSG_ID_MISSION_ACK, &wpa); + + if (_verbose) { warnx("WPM: Send MISSION_ACK type %u to ID %u", wpa.type, wpa.target_system); } +} + + +void +MavlinkMissionManager::send_mission_current(uint16_t seq) +{ + if (seq < _count) { + mavlink_mission_current_t wpc; + + wpc.seq = seq; + + _mavlink->send_message(MAVLINK_MSG_ID_MISSION_CURRENT, &wpc); + + } else if (seq == 0 && _count == 0) { + /* don't broadcast if no WPs */ + + } else { + if (_verbose) { warnx("WPM: Send MISSION_CURRENT ERROR: seq %u out of bounds", seq); } + + _mavlink->send_statustext_critical("ERROR: wp index out of bounds"); + } +} + + +void +MavlinkMissionManager::send_mission_count(uint8_t sysid, uint8_t compid, uint16_t count) +{ + _time_last_sent = hrt_absolute_time(); + + mavlink_mission_count_t wpc; + + wpc.target_system = sysid; + wpc.target_component = compid; + wpc.count = _count; + + _mavlink->send_message(MAVLINK_MSG_ID_MISSION_COUNT, &wpc); + + if (_verbose) { warnx("WPM: Send MISSION_COUNT %u to ID %u", wpc.count, wpc.target_system); } +} + + +void +MavlinkMissionManager::send_mission_item(uint8_t sysid, uint8_t compid, uint16_t seq) +{ + dm_item_t dm_item = DM_KEY_WAYPOINTS_OFFBOARD(_dataman_id); + struct mission_item_s mission_item; + + if (dm_read(dm_item, seq, &mission_item, sizeof(struct mission_item_s)) == sizeof(struct mission_item_s)) { + _time_last_sent = hrt_absolute_time(); + + /* create mission_item_s from mavlink_mission_item_t */ + mavlink_mission_item_t wp; + format_mavlink_mission_item(&mission_item, &wp); + + wp.target_system = sysid; + wp.target_component = compid; + wp.seq = seq; + wp.current = (_current_seq == seq) ? 1 : 0; + + _mavlink->send_message(MAVLINK_MSG_ID_MISSION_ITEM, &wp); + + if (_verbose) { warnx("WPM: Send MISSION_ITEM seq %u to ID %u", wp.seq, wp.target_system); } + + } else { + send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ERROR); + _mavlink->send_statustext_critical("Unable to read from micro SD"); + + if (_verbose) { warnx("WPM: Send MISSION_ITEM ERROR: could not read seq %u from dataman ID %i", seq, _dataman_id); } + } +} + + +void +MavlinkMissionManager::send_mission_request(uint8_t sysid, uint8_t compid, uint16_t seq) +{ + if (seq < _max_count) { + _time_last_sent = hrt_absolute_time(); + + mavlink_mission_request_t wpr; + wpr.target_system = sysid; + wpr.target_component = compid; + wpr.seq = seq; + + _mavlink->send_message(MAVLINK_MSG_ID_MISSION_REQUEST, &wpr); + + if (_verbose) { warnx("WPM: Send MISSION_REQUEST seq %u to ID %u", wpr.seq, wpr.target_system); } + + } else { + _mavlink->send_statustext_critical("ERROR: Waypoint index exceeds list capacity"); + + if (_verbose) { warnx("WPM: Send MISSION_REQUEST ERROR: seq %u exceeds list capacity", seq); } + } +} + + +void +MavlinkMissionManager::send_mission_item_reached(uint16_t seq) +{ + mavlink_mission_item_reached_t wp_reached; + + wp_reached.seq = seq; + + _mavlink->send_message(MAVLINK_MSG_ID_MISSION_ITEM_REACHED, &wp_reached); + + if (_verbose) { warnx("WPM: Send MISSION_ITEM_REACHED reached_seq %u", wp_reached.seq); } +} + + +void +MavlinkMissionManager::send(const hrt_abstime now) +{ + /* update interval for slow rate limiter */ + _slow_rate_limiter.set_interval(_interval * 10 / _mavlink->get_rate_mult()); + + bool updated = false; + orb_check(_mission_result_sub, &updated); + + if (updated) { + mission_result_s mission_result; + orb_copy(ORB_ID(mission_result), _mission_result_sub, &mission_result); + + _current_seq = mission_result.seq_current; + + if (_verbose) { warnx("WPM: got mission result, new current_seq: %d", _current_seq); } + + if (mission_result.reached) { + send_mission_item_reached((uint16_t)mission_result.seq_reached); + } + + send_mission_current(_current_seq); + + } else { + if (_slow_rate_limiter.check(now)) { + send_mission_current(_current_seq); + } + } + + /* check for timed-out operations */ + if (_state != MAVLINK_WPM_STATE_IDLE && hrt_elapsed_time(&_time_last_recv) > _action_timeout) { + _mavlink->send_statustext_critical("Operation timeout"); + + if (_verbose) { warnx("WPM: Last operation (state=%u) timed out, changing state to MAVLINK_WPM_STATE_IDLE", _state); } + + _state = MAVLINK_WPM_STATE_IDLE; + + } else if (_state == MAVLINK_WPM_STATE_GETLIST && hrt_elapsed_time(&_time_last_sent) > _retry_timeout) { + /* try to request item again after timeout */ + send_mission_request(_transfer_partner_sysid, _transfer_partner_compid, _transfer_seq); + + } else if (_state == MAVLINK_WPM_STATE_SENDLIST && hrt_elapsed_time(&_time_last_sent) > _retry_timeout) { + if (_transfer_seq == 0) { + /* try to send items count again after timeout */ + send_mission_count(_transfer_partner_sysid, _transfer_partner_compid, _transfer_count); + + } else { + /* try to send item again after timeout */ + send_mission_item(_transfer_partner_sysid, _transfer_partner_compid, _transfer_seq - 1); + } + } +} + + +void +MavlinkMissionManager::handle_message(const mavlink_message_t *msg) +{ + switch (msg->msgid) { + case MAVLINK_MSG_ID_MISSION_ACK: + handle_mission_ack(msg); + break; + + case MAVLINK_MSG_ID_MISSION_SET_CURRENT: + handle_mission_set_current(msg); + break; + + case MAVLINK_MSG_ID_MISSION_REQUEST_LIST: + handle_mission_request_list(msg); + break; + + case MAVLINK_MSG_ID_MISSION_REQUEST: + handle_mission_request(msg); + break; + + case MAVLINK_MSG_ID_MISSION_COUNT: + handle_mission_count(msg); + break; + + case MAVLINK_MSG_ID_MISSION_ITEM: + handle_mission_item(msg); + break; + + case MAVLINK_MSG_ID_MISSION_CLEAR_ALL: + handle_mission_clear_all(msg); + break; + + default: + break; + } +} + + +void +MavlinkMissionManager::handle_mission_ack(const mavlink_message_t *msg) +{ + mavlink_mission_ack_t wpa; + mavlink_msg_mission_ack_decode(msg, &wpa); + + if (wpa.target_system == mavlink_system.sysid /*&& wpa.target_component == mavlink_wpm_comp_id*/) { + if ((msg->sysid == _transfer_partner_sysid && msg->compid == _transfer_partner_compid)) { + if (_state == MAVLINK_WPM_STATE_SENDLIST) { + _time_last_recv = hrt_absolute_time(); + + if (_transfer_seq == _count) { + if (_verbose) { warnx("WPM: MISSION_ACK OK all items sent, switch to state IDLE"); } + + } else { + _mavlink->send_statustext_critical("WPM: ERR: not all items sent -> IDLE"); + + if (_verbose) { warnx("WPM: MISSION_ACK ERROR: not all items sent, switch to state IDLE anyway"); } + } + + _state = MAVLINK_WPM_STATE_IDLE; + } + + } else { + _mavlink->send_statustext_critical("REJ. WP CMD: partner id mismatch"); + + if (_verbose) { warnx("WPM: MISSION_ACK ERROR: rejected, partner ID mismatch"); } + } + } +} + + +void +MavlinkMissionManager::handle_mission_set_current(const mavlink_message_t *msg) +{ + mavlink_mission_set_current_t wpc; + mavlink_msg_mission_set_current_decode(msg, &wpc); + + if (wpc.target_system == mavlink_system.sysid /*&& wpc.target_component == mavlink_wpm_comp_id*/) { + if (_state == MAVLINK_WPM_STATE_IDLE) { + _time_last_recv = hrt_absolute_time(); + + if (wpc.seq < _count) { + if (update_active_mission(_dataman_id, _count, wpc.seq) == OK) { + if (_verbose) { warnx("WPM: MISSION_SET_CURRENT seq=%d OK", wpc.seq); } + + } else { + if (_verbose) { warnx("WPM: MISSION_SET_CURRENT seq=%d ERROR", wpc.seq); } + + _mavlink->send_statustext_critical("WPM: WP CURR CMD: Error setting ID"); + } + + } else { + if (_verbose) { warnx("WPM: MISSION_SET_CURRENT seq=%d ERROR: not in list", wpc.seq); } + + _mavlink->send_statustext_critical("WPM: WP CURR CMD: Not in list"); + } + + } else { + if (_verbose) { warnx("WPM: MISSION_SET_CURRENT ERROR: busy"); } + + _mavlink->send_statustext_critical("WPM: IGN WP CURR CMD: Busy"); + } + } +} + + +void +MavlinkMissionManager::handle_mission_request_list(const mavlink_message_t *msg) +{ + mavlink_mission_request_list_t wprl; + mavlink_msg_mission_request_list_decode(msg, &wprl); + + if (wprl.target_system == mavlink_system.sysid /*&& wprl.target_component == mavlink_wpm_comp_id*/) { + if (_state == MAVLINK_WPM_STATE_IDLE || _state == MAVLINK_WPM_STATE_SENDLIST) { + _time_last_recv = hrt_absolute_time(); + + if (_count > 0) { + _state = MAVLINK_WPM_STATE_SENDLIST; + _transfer_seq = 0; + _transfer_count = _count; + _transfer_partner_sysid = msg->sysid; + _transfer_partner_compid = msg->compid; + + if (_verbose) { warnx("WPM: MISSION_REQUEST_LIST OK, %u mission items to send", _transfer_count); } + + } else { + if (_verbose) { warnx("WPM: MISSION_REQUEST_LIST OK nothing to send, mission is empty"); } + + _mavlink->send_statustext_info("WPM: mission is empty"); + } + + send_mission_count(msg->sysid, msg->compid, _count); + + } else { + if (_verbose) { warnx("WPM: MISSION_REQUEST_LIST ERROR: busy"); } + + _mavlink->send_statustext_critical("IGN REQUEST LIST: Busy"); + } + } +} + + +void +MavlinkMissionManager::handle_mission_request(const mavlink_message_t *msg) +{ + mavlink_mission_request_t wpr; + mavlink_msg_mission_request_decode(msg, &wpr); + + if (wpr.target_system == mavlink_system.sysid /*&& wpr.target_component == mavlink_wpm_comp_id*/) { + if (msg->sysid == _transfer_partner_sysid && msg->compid == _transfer_partner_compid) { + if (_state == MAVLINK_WPM_STATE_SENDLIST) { + _time_last_recv = hrt_absolute_time(); + + /* _transfer_seq contains sequence of expected request */ + if (wpr.seq == _transfer_seq && _transfer_seq < _transfer_count) { + if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST seq %u from ID %u", wpr.seq, msg->sysid); } + + _transfer_seq++; + + } else if (wpr.seq == _transfer_seq - 1) { + if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST seq %u from ID %u (again)", wpr.seq, msg->sysid); } + + } else { + if (_transfer_seq > 0 && _transfer_seq < _transfer_count) { + if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST ERROR: seq %u from ID %u unexpected, must be %i or %i", wpr.seq, msg->sysid, _transfer_seq - 1, _transfer_seq); } + + } else if (_transfer_seq <= 0) { + if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST ERROR: seq %u from ID %u unexpected, must be %i", wpr.seq, msg->sysid, _transfer_seq); } + + } else { + if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST ERROR: seq %u from ID %u unexpected, must be %i", wpr.seq, msg->sysid, _transfer_seq - 1); } + } + + _state = MAVLINK_WPM_STATE_IDLE; + + send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ERROR); + _mavlink->send_statustext_critical("WPM: REJ. CMD: Req. WP was unexpected"); + return; + } + + /* double check bounds in case of items count changed */ + if (wpr.seq < _count) { + send_mission_item(_transfer_partner_sysid, _transfer_partner_compid, wpr.seq); + + } else { + if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST ERROR: seq %u out of bound [%u, %u]", (unsigned)wpr.seq, (unsigned)wpr.seq, (unsigned)_count - 1); } + + _state = MAVLINK_WPM_STATE_IDLE; + + send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ERROR); + _mavlink->send_statustext_critical("WPM: REJ. CMD: Req. WP was unexpected"); + } + + } else if (_state == MAVLINK_WPM_STATE_IDLE) { + if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST ERROR: no transfer"); } + + _mavlink->send_statustext_critical("IGN MISSION_ITEM_REQUEST: No active transfer"); + + } else { + if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST ERROR: busy (state %d).", _state); } + + _mavlink->send_statustext_critical("WPM: REJ. CMD: Busy"); + } + + } else { + _mavlink->send_statustext_critical("WPM: REJ. CMD: partner id mismatch"); + + if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST ERROR: rejected, partner ID mismatch"); } + } + } +} + + +void +MavlinkMissionManager::handle_mission_count(const mavlink_message_t *msg) +{ + mavlink_mission_count_t wpc; + mavlink_msg_mission_count_decode(msg, &wpc); + + if (wpc.target_system == mavlink_system.sysid/* && wpc.target_component == mavlink_wpm_comp_id*/) { + if (_state == MAVLINK_WPM_STATE_IDLE) { + _time_last_recv = hrt_absolute_time(); + + if (wpc.count > _max_count) { + if (_verbose) { warnx("WPM: MISSION_COUNT ERROR: too many waypoints (%d), supported: %d", wpc.count, _max_count); } + + send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_NO_SPACE); + return; + } + + if (wpc.count == 0) { + if (_verbose) { warnx("WPM: MISSION_COUNT 0, clearing waypoints list and staying in state MAVLINK_WPM_STATE_IDLE"); } + + /* alternate dataman ID anyway to let navigator know about changes */ + update_active_mission(_dataman_id == 0 ? 1 : 0, 0, 0); + _mavlink->send_statustext_info("WPM: COUNT 0: CLEAR MISSION"); + + // TODO send ACK? + return; + } + + if (_verbose) { warnx("WPM: MISSION_COUNT %u from ID %u, changing state to MAVLINK_WPM_STATE_GETLIST", wpc.count, msg->sysid); } + + _state = MAVLINK_WPM_STATE_GETLIST; + _transfer_seq = 0; + _transfer_partner_sysid = msg->sysid; + _transfer_partner_compid = msg->compid; + _transfer_count = wpc.count; + _transfer_dataman_id = _dataman_id == 0 ? 1 : 0; // use inactive storage for transmission + _transfer_current_seq = -1; + + } else if (_state == MAVLINK_WPM_STATE_GETLIST) { + _time_last_recv = hrt_absolute_time(); + + if (_transfer_seq == 0) { + /* looks like our MISSION_REQUEST was lost, try again */ + if (_verbose) { warnx("WPM: MISSION_COUNT %u from ID %u (again)", wpc.count, msg->sysid); } + + _mavlink->send_statustext_info("WP CMD OK TRY AGAIN"); + + } else { + if (_verbose) { warnx("WPM: MISSION_COUNT ERROR: busy, already receiving seq %u", _transfer_seq); } + + _mavlink->send_statustext_critical("WPM: REJ. CMD: Busy"); + return; + } + + } else { + if (_verbose) { warnx("WPM: MISSION_COUNT ERROR: busy, state %i", _state); } + + _mavlink->send_statustext_critical("WPM: IGN MISSION_COUNT: Busy"); + return; + } + + send_mission_request(_transfer_partner_sysid, _transfer_partner_compid, _transfer_seq); + } +} + + +void +MavlinkMissionManager::handle_mission_item(const mavlink_message_t *msg) +{ + mavlink_mission_item_t wp; + mavlink_msg_mission_item_decode(msg, &wp); + + if (wp.target_system == mavlink_system.sysid && wp.target_component == _comp_id) { + if (_state == MAVLINK_WPM_STATE_GETLIST) { + _time_last_recv = hrt_absolute_time(); + + if (wp.seq != _transfer_seq) { + if (_verbose) { warnx("WPM: MISSION_ITEM ERROR: seq %u was not the expected %u", wp.seq, _transfer_seq); } + + /* don't send request here, it will be performed in eventloop after timeout */ + return; + } + + } else if (_state == MAVLINK_WPM_STATE_IDLE) { + if (_verbose) { warnx("WPM: MISSION_ITEM ERROR: no transfer"); } + + _mavlink->send_statustext_critical("IGN MISSION_ITEM: No transfer"); + return; + + } else { + if (_verbose) { warnx("WPM: MISSION_ITEM ERROR: busy, state %i", _state); } + + _mavlink->send_statustext_critical("IGN MISSION_ITEM: Busy"); + return; + } + + struct mission_item_s mission_item; + int ret = parse_mavlink_mission_item(&wp, &mission_item); + + if (ret != OK) { + if (_verbose) { warnx("WPM: MISSION_ITEM ERROR: seq %u invalid item", wp.seq); } + + _mavlink->send_statustext_critical("IGN MISSION_ITEM: Busy"); + + send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, ret); + _state = MAVLINK_WPM_STATE_IDLE; + return; + } + + dm_item_t dm_item = DM_KEY_WAYPOINTS_OFFBOARD(_transfer_dataman_id); + + if (dm_write(dm_item, wp.seq, DM_PERSIST_POWER_ON_RESET, &mission_item, sizeof(struct mission_item_s)) != sizeof(struct mission_item_s)) { + if (_verbose) { warnx("WPM: MISSION_ITEM ERROR: error writing seq %u to dataman ID %i", wp.seq, _transfer_dataman_id); } + + send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ERROR); + _mavlink->send_statustext_critical("Unable to write on micro SD"); + _state = MAVLINK_WPM_STATE_IDLE; + return; + } + + /* waypoint marked as current */ + if (wp.current) { + _transfer_current_seq = wp.seq; + } + + if (_verbose) { warnx("WPM: MISSION_ITEM seq %u received", wp.seq); } + + _transfer_seq = wp.seq + 1; + + if (_transfer_seq == _transfer_count) { + /* got all new mission items successfully */ + if (_verbose) { warnx("WPM: MISSION_ITEM got all %u items, current_seq=%u, changing state to MAVLINK_WPM_STATE_IDLE", _transfer_count, _transfer_current_seq); } + + _mavlink->send_statustext_info("WPM: Transfer complete."); + + _state = MAVLINK_WPM_STATE_IDLE; + + if (update_active_mission(_transfer_dataman_id, _transfer_count, _transfer_current_seq) == OK) { + send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ACCEPTED); + + } else { + send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ERROR); + } + + } else { + /* request next item */ + send_mission_request(_transfer_partner_sysid, _transfer_partner_compid, _transfer_seq); + } + } +} + + +void +MavlinkMissionManager::handle_mission_clear_all(const mavlink_message_t *msg) +{ + mavlink_mission_clear_all_t wpca; + mavlink_msg_mission_clear_all_decode(msg, &wpca); + + if (wpca.target_system == mavlink_system.sysid /*&& wpca.target_component == mavlink_wpm_comp_id */) { + + if (_state == MAVLINK_WPM_STATE_IDLE) { + /* don't touch mission items storage itself, but only items count in mission state */ + _time_last_recv = hrt_absolute_time(); + + if (update_active_mission(_dataman_id == 0 ? 1 : 0, 0, 0) == OK) { + if (_verbose) { warnx("WPM: CLEAR_ALL OK"); } + send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ACCEPTED); + + } else { + send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ERROR); + } + + } else { + _mavlink->send_statustext_critical("WPM: IGN CLEAR CMD: Busy"); + + if (_verbose) { warnx("WPM: CLEAR_ALL IGNORED: busy"); } + } + } +} + +int +MavlinkMissionManager::parse_mavlink_mission_item(const mavlink_mission_item_t *mavlink_mission_item, struct mission_item_s *mission_item) +{ + /* only support global waypoints for now */ + switch (mavlink_mission_item->frame) { + case MAV_FRAME_GLOBAL: + mission_item->lat = (double)mavlink_mission_item->x; + mission_item->lon = (double)mavlink_mission_item->y; + mission_item->altitude = mavlink_mission_item->z; + mission_item->altitude_is_relative = false; + break; + + case MAV_FRAME_GLOBAL_RELATIVE_ALT: + mission_item->lat = (double)mavlink_mission_item->x; + mission_item->lon = (double)mavlink_mission_item->y; + mission_item->altitude = mavlink_mission_item->z; + mission_item->altitude_is_relative = true; + break; + + case MAV_FRAME_LOCAL_NED: + case MAV_FRAME_LOCAL_ENU: + return MAV_MISSION_UNSUPPORTED_FRAME; + + case MAV_FRAME_MISSION: + default: + return MAV_MISSION_ERROR; + } + + switch (mavlink_mission_item->command) { + case MAV_CMD_NAV_TAKEOFF: + mission_item->pitch_min = mavlink_mission_item->param1; + break; + case MAV_CMD_DO_JUMP: + mission_item->do_jump_mission_index = mavlink_mission_item->param1; + mission_item->do_jump_current_count = 0; + mission_item->do_jump_repeat_count = mavlink_mission_item->param2; + break; + default: + mission_item->acceptance_radius = mavlink_mission_item->param2; + mission_item->time_inside = mavlink_mission_item->param1; + break; + } + + mission_item->yaw = _wrap_pi(mavlink_mission_item->param4 * M_DEG_TO_RAD_F); + mission_item->loiter_radius = fabsf(mavlink_mission_item->param3); + mission_item->loiter_direction = (mavlink_mission_item->param3 > 0) ? 1 : -1; /* 1 if positive CW, -1 if negative CCW */ + mission_item->nav_cmd = (NAV_CMD)mavlink_mission_item->command; + + mission_item->autocontinue = mavlink_mission_item->autocontinue; + // mission_item->index = mavlink_mission_item->seq; + mission_item->origin = ORIGIN_MAVLINK; + + /* reset DO_JUMP count */ + mission_item->do_jump_current_count = 0; + + return MAV_MISSION_ACCEPTED; +} + + +int +MavlinkMissionManager::format_mavlink_mission_item(const struct mission_item_s *mission_item, mavlink_mission_item_t *mavlink_mission_item) +{ + if (mission_item->altitude_is_relative) { + mavlink_mission_item->frame = MAV_FRAME_GLOBAL; + + } else { + mavlink_mission_item->frame = MAV_FRAME_GLOBAL_RELATIVE_ALT; + } + + switch (mission_item->nav_cmd) { + case NAV_CMD_TAKEOFF: + mavlink_mission_item->param1 = mission_item->pitch_min; + break; + + case NAV_CMD_DO_JUMP: + mavlink_mission_item->param1 = mission_item->do_jump_mission_index; + mavlink_mission_item->param2 = mission_item->do_jump_repeat_count; + break; + + default: + mavlink_mission_item->param2 = mission_item->acceptance_radius; + mavlink_mission_item->param1 = mission_item->time_inside; + break; + } + + mavlink_mission_item->x = (float)mission_item->lat; + mavlink_mission_item->y = (float)mission_item->lon; + mavlink_mission_item->z = mission_item->altitude; + + mavlink_mission_item->param4 = mission_item->yaw * M_RAD_TO_DEG_F; + mavlink_mission_item->param3 = mission_item->loiter_radius * (float)mission_item->loiter_direction; + mavlink_mission_item->command = mission_item->nav_cmd; + mavlink_mission_item->autocontinue = mission_item->autocontinue; + // mavlink_mission_item->seq = mission_item->index; + + return OK; +} diff --git a/src/modules/mavlink/mavlink_mission.h b/src/modules/mavlink/mavlink_mission.h new file mode 100644 index 000000000..8ff27f87d --- /dev/null +++ b/src/modules/mavlink/mavlink_mission.h @@ -0,0 +1,198 @@ +/**************************************************************************** + * + * 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 mavlink_mission.h + * MAVLink mission manager interface definition. + * + * @author Lorenz Meier <lm@inf.ethz.ch> + * @author Julian Oes <joes@student.ethz.ch> + * @author Anton Babushkin <anton.babushkin@me.com> + */ + +#pragma once + +#include <uORB/uORB.h> + +#include "mavlink_bridge_header.h" +#include "mavlink_rate_limiter.h" +#include "mavlink_stream.h" + +enum MAVLINK_WPM_STATES { + MAVLINK_WPM_STATE_IDLE = 0, + MAVLINK_WPM_STATE_SENDLIST, + MAVLINK_WPM_STATE_GETLIST, + MAVLINK_WPM_STATE_ENUM_END +}; + +enum MAVLINK_WPM_CODES { + MAVLINK_WPM_CODE_OK = 0, + MAVLINK_WPM_CODE_ERR_WAYPOINT_ACTION_NOT_SUPPORTED, + MAVLINK_WPM_CODE_ERR_WAYPOINT_FRAME_NOT_SUPPORTED, + MAVLINK_WPM_CODE_ERR_WAYPOINT_OUT_OF_BOUNDS, + MAVLINK_WPM_CODE_ERR_WAYPOINT_MAX_NUMBER_EXCEEDED, + MAVLINK_WPM_CODE_ENUM_END +}; + +#define MAVLINK_MISSION_PROTOCOL_TIMEOUT_DEFAULT 5000000 ///< Protocol communication action timeout in useconds +#define MAVLINK_MISSION_RETRY_TIMEOUT_DEFAULT 500000 ///< Protocol communication retry timeout in useconds + +class MavlinkMissionManager : public MavlinkStream { +public: + ~MavlinkMissionManager(); + + const char *get_name() const + { + return MavlinkMissionManager::get_name_static(); + } + + static const char *get_name_static() + { + return "MISSION_ITEM"; + } + + uint8_t get_id() + { + return MAVLINK_MSG_ID_MISSION_ITEM; + } + + static MavlinkStream *new_instance(Mavlink *mavlink) + { + return new MavlinkMissionManager(mavlink); + } + + unsigned get_size(); + + void handle_message(const mavlink_message_t *msg); + + void set_verbose(bool v) { _verbose = v; } + +private: + enum MAVLINK_WPM_STATES _state; ///< Current state + + uint64_t _time_last_recv; + uint64_t _time_last_sent; + + uint32_t _action_timeout; + uint32_t _retry_timeout; + unsigned _max_count; ///< Maximum number of mission items + + int _dataman_id; ///< Dataman storage ID for active mission + unsigned _count; ///< Count of items in active mission + int _current_seq; ///< Current item sequence in active mission + + int _transfer_dataman_id; ///< Dataman storage ID for current transmission + unsigned _transfer_count; ///< Items count in current transmission + unsigned _transfer_seq; ///< Item sequence in current transmission + unsigned _transfer_current_seq; ///< Current item ID for current transmission (-1 means not initialized) + unsigned _transfer_partner_sysid; ///< Partner system ID for current transmission + unsigned _transfer_partner_compid; ///< Partner component ID for current transmission + + int _offboard_mission_sub; + int _mission_result_sub; + orb_advert_t _offboard_mission_pub; + + MavlinkRateLimiter _slow_rate_limiter; + + bool _verbose; + + uint8_t _comp_id; + + /* do not allow top copying this class */ + MavlinkMissionManager(MavlinkMissionManager &); + MavlinkMissionManager& operator = (const MavlinkMissionManager &); + + void init_offboard_mission(); + + int update_active_mission(int dataman_id, unsigned count, int seq); + + /** + * @brief Sends an waypoint ack message + */ + void send_mission_ack(uint8_t sysid, uint8_t compid, uint8_t type); + + /** + * @brief Broadcasts the new target waypoint and directs the MAV to fly there + * + * This function broadcasts its new active waypoint sequence number and + * sends a message to the controller, advising it to fly to the coordinates + * of the waypoint with a given orientation + * + * @param seq The waypoint sequence number the MAV should fly to. + */ + void send_mission_current(uint16_t seq); + + void send_mission_count(uint8_t sysid, uint8_t compid, uint16_t count); + + void send_mission_item(uint8_t sysid, uint8_t compid, uint16_t seq); + + void send_mission_request(uint8_t sysid, uint8_t compid, uint16_t seq); + + /** + * @brief emits a message that a waypoint reached + * + * This function broadcasts a message that a waypoint is reached. + * + * @param seq The waypoint sequence number the MAV has reached. + */ + void send_mission_item_reached(uint16_t seq); + + void handle_mission_ack(const mavlink_message_t *msg); + + void handle_mission_set_current(const mavlink_message_t *msg); + + void handle_mission_request_list(const mavlink_message_t *msg); + + void handle_mission_request(const mavlink_message_t *msg); + + void handle_mission_count(const mavlink_message_t *msg); + + void handle_mission_item(const mavlink_message_t *msg); + + void handle_mission_clear_all(const mavlink_message_t *msg); + + /** + * Parse mavlink MISSION_ITEM message to get mission_item_s. + */ + int parse_mavlink_mission_item(const mavlink_mission_item_t *mavlink_mission_item, struct mission_item_s *mission_item); + + /** + * Format mission_item_s as mavlink MISSION_ITEM message. + */ + int format_mavlink_mission_item(const struct mission_item_s *mission_item, mavlink_mission_item_t *mavlink_mission_item); + +protected: + explicit MavlinkMissionManager(Mavlink *mavlink); + + void send(const hrt_abstime t); +}; diff --git a/src/modules/mavlink/mavlink_orb_subscription.h b/src/modules/mavlink/mavlink_orb_subscription.h index 71efb43af..7af454df6 100644 --- a/src/modules/mavlink/mavlink_orb_subscription.h +++ b/src/modules/mavlink/mavlink_orb_subscription.h @@ -82,6 +82,10 @@ private: const orb_id_t _topic; ///< topic metadata int _fd; ///< subscription handle bool _published; ///< topic was ever published + + /* do not allow copying this class */ + MavlinkOrbSubscription(const MavlinkOrbSubscription&); + MavlinkOrbSubscription operator=(const MavlinkOrbSubscription&); }; diff --git a/src/modules/mavlink/mavlink_parameters.cpp b/src/modules/mavlink/mavlink_parameters.cpp new file mode 100644 index 000000000..cd5f53d88 --- /dev/null +++ b/src/modules/mavlink/mavlink_parameters.cpp @@ -0,0 +1,197 @@ +/**************************************************************************** + * + * 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 mavlink_parameters.cpp + * Mavlink parameters manager implementation. + * + * @author Anton Babushkin <anton.babushkin@me.com> + */ + +#include <stdio.h> + +#include "mavlink_parameters.h" +#include "mavlink_main.h" + +MavlinkParametersManager::MavlinkParametersManager(Mavlink *mavlink) : MavlinkStream(mavlink), + _send_all_index(-1) +{ +} + +unsigned +MavlinkParametersManager::get_size() +{ + if (_send_all_index >= 0) { + return MAVLINK_MSG_ID_PARAM_VALUE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + + } else { + return 0; + } +} + +void +MavlinkParametersManager::handle_message(const mavlink_message_t *msg) +{ + switch (msg->msgid) { + case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: { + /* request all parameters */ + mavlink_param_request_list_t req_list; + mavlink_msg_param_request_list_decode(msg, &req_list); + + if (req_list.target_system == mavlink_system.sysid && + (req_list.target_component == mavlink_system.compid || req_list.target_component == MAV_COMP_ID_ALL)) { + + _send_all_index = 0; + _mavlink->send_statustext_info("[pm] sending list"); + } + break; + } + + case MAVLINK_MSG_ID_PARAM_SET: { + /* set parameter */ + if (msg->msgid == MAVLINK_MSG_ID_PARAM_SET) { + mavlink_param_set_t set; + mavlink_msg_param_set_decode(msg, &set); + + if (set.target_system == mavlink_system.sysid && + (set.target_component == mavlink_system.compid || set.target_component == MAV_COMP_ID_ALL)) { + + /* local name buffer to enforce null-terminated string */ + char name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN + 1]; + strncpy(name, set.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN); + /* enforce null termination */ + name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN] = '\0'; + /* attempt to find parameter, set and send it */ + param_t param = param_find(name); + + if (param == PARAM_INVALID) { + char buf[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN]; + sprintf(buf, "[pm] unknown param: %s", name); + _mavlink->send_statustext_info(buf); + + } else { + /* set and send parameter */ + param_set(param, &(set.param_value)); + send_param(param); + } + } + } + break; + } + + case MAVLINK_MSG_ID_PARAM_REQUEST_READ: { + /* request one parameter */ + mavlink_param_request_read_t req_read; + mavlink_msg_param_request_read_decode(msg, &req_read); + + if (req_read.target_system == mavlink_system.sysid && + (req_read.target_component == mavlink_system.compid || req_read.target_component == MAV_COMP_ID_ALL)) { + + /* when no index is given, loop through string ids and compare them */ + if (req_read.param_index < 0) { + /* local name buffer to enforce null-terminated string */ + char name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN + 1]; + strncpy(name, req_read.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN); + /* enforce null termination */ + name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN] = '\0'; + /* attempt to find parameter and send it */ + send_param(param_find(name)); + + } else { + /* when index is >= 0, send this parameter again */ + send_param(param_for_index(req_read.param_index)); + } + } + break; + } + + default: + break; + } +} + +void +MavlinkParametersManager::send(const hrt_abstime t) +{ + /* send all parameters if requested */ + if (_send_all_index >= 0) { + send_param(param_for_index(_send_all_index)); + _send_all_index++; + if (_send_all_index >= (int) param_count()) { + _send_all_index = -1; + } + } +} + +void +MavlinkParametersManager::send_param(param_t param) +{ + if (param == PARAM_INVALID) { + return; + } + + mavlink_param_value_t msg; + + /* + * get param value, since MAVLink encodes float and int params in the same + * space during transmission, copy param onto float val_buf + */ + if (param_get(param, &msg.param_value) != OK) { + return; + } + + msg.param_count = param_count(); + msg.param_index = param_get_index(param); + + /* copy parameter name */ + strncpy(msg.param_id, param_name(param), MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN); + + /* query parameter type */ + param_type_t type = param_type(param); + + /* + * Map onboard parameter type to MAVLink type, + * endianess matches (both little endian) + */ + if (type == PARAM_TYPE_INT32) { + msg.param_type = MAVLINK_TYPE_INT32_T; + + } else if (type == PARAM_TYPE_FLOAT) { + msg.param_type = MAVLINK_TYPE_FLOAT; + + } else { + msg.param_type = MAVLINK_TYPE_FLOAT; + } + + _mavlink->send_message(MAVLINK_MSG_ID_PARAM_VALUE, &msg); +} diff --git a/src/modules/mavlink/mavlink_parameters.h b/src/modules/mavlink/mavlink_parameters.h new file mode 100644 index 000000000..5576e6b84 --- /dev/null +++ b/src/modules/mavlink/mavlink_parameters.h @@ -0,0 +1,115 @@ +/**************************************************************************** + * + * 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 mavlink_parameters.h + * Mavlink parameters manager definition. + * + * @author Anton Babushkin <anton.babushkin@me.com> + */ + +#pragma once + +#include <systemlib/param/param.h> + +#include "mavlink_bridge_header.h" +#include "mavlink_stream.h" + +class MavlinkParametersManager : public MavlinkStream +{ +public: + const char *get_name() const + { + return MavlinkParametersManager::get_name_static(); + } + + static const char *get_name_static() + { + return "PARAM_VALUE"; + } + + uint8_t get_id() + { + return MAVLINK_MSG_ID_PARAM_VALUE; + } + + static MavlinkStream *new_instance(Mavlink *mavlink) + { + return new MavlinkParametersManager(mavlink); + } + + unsigned get_size(); + + void handle_message(const mavlink_message_t *msg); + + /** + * Send one parameter identified by index. + * + * @param index The index of the parameter to send. + * @return zero on success, nonzero else. + */ + void start_send_one(int index); + + + /** + * Send one parameter identified by name. + * + * @param name The index of the parameter to send. + * @return zero on success, nonzero else. + */ + int start_send_for_name(const char *name); + + /** + * Start sending the parameter queue. + * + * This function will not directly send parameters, but instead + * activate the sending of one parameter on each call of + * mavlink_pm_queued_send(). + * @see mavlink_pm_queued_send() + */ + void start_send_all(); + +private: + int _send_all_index; + + /* do not allow top copying this class */ + MavlinkParametersManager(MavlinkParametersManager &); + MavlinkParametersManager& operator = (const MavlinkParametersManager &); + +protected: + explicit MavlinkParametersManager(Mavlink *mavlink); + + void send(const hrt_abstime t); + + void send_param(param_t param); +}; diff --git a/src/modules/mavlink/mavlink_rate_limiter.cpp b/src/modules/mavlink/mavlink_rate_limiter.cpp index 9cdda8b17..d3b3e1cde 100644 --- a/src/modules/mavlink/mavlink_rate_limiter.cpp +++ b/src/modules/mavlink/mavlink_rate_limiter.cpp @@ -64,7 +64,7 @@ MavlinkRateLimiter::check(hrt_abstime t) uint64_t dt = t - _last_sent; if (dt > 0 && dt >= _interval) { - _last_sent = (t / _interval) * _interval; + _last_sent = t; return true; } diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index bb977d277..63bac45c0 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -79,7 +79,6 @@ __BEGIN_DECLS #include "mavlink_bridge_header.h" #include "mavlink_receiver.h" #include "mavlink_main.h" -#include "util.h" __END_DECLS @@ -87,7 +86,9 @@ static const float mg2ms2 = CONSTANTS_ONE_G / 1000.0f; MavlinkReceiver::MavlinkReceiver(Mavlink *parent) : _mavlink(parent), - + status{}, + hil_local_pos{}, + _control_mode{}, _global_pos_pub(-1), _local_pos_pub(-1), _attitude_pub(-1), @@ -102,18 +103,22 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) : _cmd_pub(-1), _flow_pub(-1), _offboard_control_sp_pub(-1), + _local_pos_sp_pub(-1), + _global_vel_sp_pub(-1), + _att_sp_pub(-1), + _rates_sp_pub(-1), _vicon_position_pub(-1), _telemetry_status_pub(-1), _rc_pub(-1), _manual_pub(-1), - _telemetry_heartbeat_time(0), _radio_status_available(false), + _control_mode_sub(orb_subscribe(ORB_ID(vehicle_control_mode))), _hil_frames(0), _old_timestamp(0), _hil_local_proj_inited(0), - _hil_local_alt0(0.0) + _hil_local_alt0(0.0f), + _hil_local_proj_ref{} { - memset(&hil_local_pos, 0, sizeof(hil_local_pos)); // make sure the FTP server is started (void)MavlinkFTP::getServer(); @@ -143,10 +148,6 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) handle_message_vicon_position_estimate(msg); break; - case MAVLINK_MSG_ID_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST: - handle_message_quad_swarm_roll_pitch_yaw_thrust(msg); - break; - case MAVLINK_MSG_ID_RADIO_STATUS: handle_message_radio_status(msg); break; @@ -236,7 +237,8 @@ MavlinkReceiver::handle_message_command_long(mavlink_message_t *msg) } else { if (msg->sysid == mavlink_system.sysid && msg->compid == mavlink_system.compid) { - warnx("ignoring CMD spoofed with same SYS/COMP ID"); + warnx("ignoring CMD spoofed with same SYS/COMP (%d/%d) ID", + mavlink_system.sysid, mavlink_system.compid); return; } @@ -357,96 +359,37 @@ MavlinkReceiver::handle_message_vicon_position_estimate(mavlink_message_t *msg) } void -MavlinkReceiver::handle_message_quad_swarm_roll_pitch_yaw_thrust(mavlink_message_t *msg) +MavlinkReceiver::handle_message_radio_status(mavlink_message_t *msg) { - mavlink_set_quad_swarm_roll_pitch_yaw_thrust_t quad_motors_setpoint; - mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_decode(msg, &quad_motors_setpoint); - - if (mavlink_system.sysid < 4) { - struct offboard_control_setpoint_s offboard_control_sp; - memset(&offboard_control_sp, 0, sizeof(offboard_control_sp)); - - uint8_t ml_mode = 0; - bool ml_armed = false; - - switch (quad_motors_setpoint.mode) { - case 0: - ml_armed = false; - break; - - case 1: - ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_RATES; - ml_armed = true; - - break; - - case 2: - ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE; - ml_armed = true; - - break; - - case 3: - ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY; - break; - - case 4: - ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_POSITION; - break; - } - - offboard_control_sp.p1 = (float)quad_motors_setpoint.roll[mavlink_system.sysid - 1] / (float)INT16_MAX; - offboard_control_sp.p2 = (float)quad_motors_setpoint.pitch[mavlink_system.sysid - 1] / (float)INT16_MAX; - offboard_control_sp.p3 = (float)quad_motors_setpoint.yaw[mavlink_system.sysid - 1] / (float)INT16_MAX; - offboard_control_sp.p4 = (float)quad_motors_setpoint.thrust[mavlink_system.sysid - 1] / (float)UINT16_MAX; - - if (quad_motors_setpoint.thrust[mavlink_system.sysid - 1] == 0) { - ml_armed = false; - } - - offboard_control_sp.armed = ml_armed; - offboard_control_sp.mode = static_cast<enum OFFBOARD_CONTROL_MODE>(ml_mode); - - offboard_control_sp.timestamp = hrt_absolute_time(); - - if (_offboard_control_sp_pub < 0) { - _offboard_control_sp_pub = orb_advertise(ORB_ID(offboard_control_setpoint), &offboard_control_sp); + /* telemetry status supported only on first TELEMETRY_STATUS_ORB_ID_NUM mavlink channels */ + if (_mavlink->get_channel() < TELEMETRY_STATUS_ORB_ID_NUM) { + mavlink_radio_status_t rstatus; + mavlink_msg_radio_status_decode(msg, &rstatus); + + struct telemetry_status_s &tstatus = _mavlink->get_rx_status(); + + tstatus.timestamp = hrt_absolute_time(); + tstatus.telem_time = tstatus.timestamp; + /* tstatus.heartbeat_time is set by system heartbeats */ + tstatus.type = TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO; + tstatus.rssi = rstatus.rssi; + tstatus.remote_rssi = rstatus.remrssi; + tstatus.txbuf = rstatus.txbuf; + tstatus.noise = rstatus.noise; + tstatus.remote_noise = rstatus.remnoise; + tstatus.rxerrors = rstatus.rxerrors; + tstatus.fixed = rstatus.fixed; + + if (_telemetry_status_pub < 0) { + _telemetry_status_pub = orb_advertise(telemetry_status_orb_id[_mavlink->get_channel()], &tstatus); } else { - orb_publish(ORB_ID(offboard_control_setpoint), _offboard_control_sp_pub, &offboard_control_sp); + orb_publish(telemetry_status_orb_id[_mavlink->get_channel()], _telemetry_status_pub, &tstatus); } - } -} - -void -MavlinkReceiver::handle_message_radio_status(mavlink_message_t *msg) -{ - mavlink_radio_status_t rstatus; - mavlink_msg_radio_status_decode(msg, &rstatus); - - struct telemetry_status_s tstatus; - memset(&tstatus, 0, sizeof(tstatus)); - - tstatus.timestamp = hrt_absolute_time(); - tstatus.heartbeat_time = _telemetry_heartbeat_time; - tstatus.type = TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO; - tstatus.rssi = rstatus.rssi; - tstatus.remote_rssi = rstatus.remrssi; - tstatus.txbuf = rstatus.txbuf; - tstatus.noise = rstatus.noise; - tstatus.remote_noise = rstatus.remnoise; - tstatus.rxerrors = rstatus.rxerrors; - tstatus.fixed = rstatus.fixed; - - if (_telemetry_status_pub < 0) { - _telemetry_status_pub = orb_advertise(ORB_ID(telemetry_status), &tstatus); - } else { - orb_publish(ORB_ID(telemetry_status), _telemetry_status_pub, &tstatus); + /* this means that heartbeats alone won't be published to the radio status no more */ + _radio_status_available = true; } - - /* this means that heartbeats alone won't be published to the radio status no more */ - _radio_status_available = true; } void @@ -464,6 +407,8 @@ MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg) manual.r = man.r / 1000.0f; manual.z = man.z / 1000.0f; + warnx("pitch: %.2f, roll: %.2f, yaw: %.2f, throttle: %.2f", (double)manual.x, (double)manual.y, (double)manual.r, (double)manual.z); + if (_manual_pub < 0) { _manual_pub = orb_advertise(ORB_ID(manual_control_setpoint), &manual); @@ -475,28 +420,35 @@ MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg) void MavlinkReceiver::handle_message_heartbeat(mavlink_message_t *msg) { - mavlink_heartbeat_t hb; - mavlink_msg_heartbeat_decode(msg, &hb); + /* telemetry status supported only on first TELEMETRY_STATUS_ORB_ID_NUM mavlink channels */ + if (_mavlink->get_channel() < TELEMETRY_STATUS_ORB_ID_NUM) { + mavlink_heartbeat_t hb; + mavlink_msg_heartbeat_decode(msg, &hb); - /* ignore own heartbeats, accept only heartbeats from GCS */ - if (msg->sysid != mavlink_system.sysid && hb.type == MAV_TYPE_GCS) { - _telemetry_heartbeat_time = hrt_absolute_time(); + /* ignore own heartbeats, accept only heartbeats from GCS */ + if (msg->sysid != mavlink_system.sysid && hb.type == MAV_TYPE_GCS) { - /* if no radio status messages arrive, lets at least publish that heartbeats were received */ - if (!_radio_status_available) { + struct telemetry_status_s &tstatus = _mavlink->get_rx_status(); - struct telemetry_status_s tstatus; - memset(&tstatus, 0, sizeof(tstatus)); + hrt_abstime tnow = hrt_absolute_time(); - tstatus.timestamp = _telemetry_heartbeat_time; - tstatus.heartbeat_time = _telemetry_heartbeat_time; - tstatus.type = TELEMETRY_STATUS_RADIO_TYPE_GENERIC; + /* always set heartbeat, publish only if telemetry link not up */ + tstatus.heartbeat_time = tnow; - if (_telemetry_status_pub < 0) { - _telemetry_status_pub = orb_advertise(ORB_ID(telemetry_status), &tstatus); + /* if no radio status messages arrive, lets at least publish that heartbeats were received */ + if (!_radio_status_available) { - } else { - orb_publish(ORB_ID(telemetry_status), _telemetry_status_pub, &tstatus); + tstatus.timestamp = tnow; + /* telem_time indicates the timestamp of a telemetry status packet and we got none */ + tstatus.telem_time = 0; + tstatus.type = TELEMETRY_STATUS_RADIO_TYPE_GENERIC; + + if (_telemetry_status_pub < 0) { + _telemetry_status_pub = orb_advertise(telemetry_status_orb_id[_mavlink->get_channel()], &tstatus); + + } else { + orb_publish(telemetry_status_orb_id[_mavlink->get_channel()], _telemetry_status_pub, &tstatus); + } } } } @@ -564,10 +516,10 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg) gyro.temperature = imu.temperature; if (_gyro_pub < 0) { - _gyro_pub = orb_advertise(ORB_ID(sensor_gyro), &gyro); + _gyro_pub = orb_advertise(ORB_ID(sensor_gyro0), &gyro); } else { - orb_publish(ORB_ID(sensor_gyro), _gyro_pub, &gyro); + orb_publish(ORB_ID(sensor_gyro0), _gyro_pub, &gyro); } } @@ -586,10 +538,10 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg) accel.temperature = imu.temperature; if (_accel_pub < 0) { - _accel_pub = orb_advertise(ORB_ID(sensor_accel), &accel); + _accel_pub = orb_advertise(ORB_ID(sensor_accel0), &accel); } else { - orb_publish(ORB_ID(sensor_accel), _accel_pub, &accel); + orb_publish(ORB_ID(sensor_accel0), _accel_pub, &accel); } } @@ -607,10 +559,10 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg) mag.z = imu.zmag; if (_mag_pub < 0) { - _mag_pub = orb_advertise(ORB_ID(sensor_mag), &mag); + _mag_pub = orb_advertise(ORB_ID(sensor_mag0), &mag); } else { - orb_publish(ORB_ID(sensor_mag), _mag_pub, &mag); + orb_publish(ORB_ID(sensor_mag0), _mag_pub, &mag); } } @@ -625,10 +577,10 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg) baro.temperature = imu.temperature; if (_baro_pub < 0) { - _baro_pub = orb_advertise(ORB_ID(sensor_baro), &baro); + _baro_pub = orb_advertise(ORB_ID(sensor_baro0), &baro); } else { - orb_publish(ORB_ID(sensor_baro), _baro_pub, &baro); + orb_publish(ORB_ID(sensor_baro0), _baro_pub, &baro); } } @@ -741,7 +693,6 @@ MavlinkReceiver::handle_message_hil_gps(mavlink_message_t *msg) hil_gps.timestamp_variance = timestamp; hil_gps.s_variance_m_s = 5.0f; - hil_gps.p_variance_m = hil_gps.eph * hil_gps.eph; hil_gps.timestamp_velocity = timestamp; hil_gps.vel_m_s = (float)gps.vel * 1e-2f; // from cm/s to m/s @@ -751,9 +702,8 @@ MavlinkReceiver::handle_message_hil_gps(mavlink_message_t *msg) hil_gps.vel_ned_valid = true; hil_gps.cog_rad = _wrap_pi(gps.cog * M_DEG_TO_RAD_F * 1e-2f); - hil_gps.timestamp_satellites = timestamp; hil_gps.fix_type = gps.fix_type; - hil_gps.satellites_visible = gps.satellites_visible; + hil_gps.satellites_used = gps.satellites_visible; //TODO: rename mavlink_hil_gps_t sats visible to used? if (_gps_pub < 0) { _gps_pub = orb_advertise(ORB_ID(vehicle_gps_position), &hil_gps); @@ -904,10 +854,10 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg) accel.temperature = 25.0f; if (_accel_pub < 0) { - _accel_pub = orb_advertise(ORB_ID(sensor_accel), &accel); + _accel_pub = orb_advertise(ORB_ID(sensor_accel0), &accel); } else { - orb_publish(ORB_ID(sensor_accel), _accel_pub, &accel); + orb_publish(ORB_ID(sensor_accel0), _accel_pub, &accel); } } @@ -971,18 +921,13 @@ MavlinkReceiver::receive_thread(void *arg) /* handle generic messages and commands */ handle_message(&msg); - /* handle packet with waypoint component */ - _mavlink->mavlink_wpm_message_handler(&msg); - - /* handle packet with parameter component */ - _mavlink->mavlink_pm_message_handler(_mavlink->get_channel(), &msg); - - if (_mavlink->get_forwarding_on()) { - /* forward any messages to other mavlink instances */ - Mavlink::forward_message(&msg, _mavlink); - } + /* handle packet with parent object */ + _mavlink->handle_message(&msg); } } + + /* count received bytes */ + _mavlink->count_rxbytes(nread); } } diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index 040a07480..f4d0c52d8 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -36,6 +36,7 @@ * MAVLink 1.0 uORB listener definition * * @author Lorenz Meier <lm@inf.ethz.ch> + * @author Anton Babushkin <anton.babushkin@me.com> */ #pragma once @@ -44,6 +45,7 @@ #include <uORB/uORB.h> #include <uORB/topics/sensor_combined.h> #include <uORB/topics/rc_channels.h> +#include <uORB/topics/vehicle_control_mode.h> #include <uORB/topics/vehicle_attitude.h> #include <uORB/topics/vehicle_gps_position.h> #include <uORB/topics/vehicle_global_position.h> @@ -53,6 +55,7 @@ #include <uORB/topics/offboard_control_setpoint.h> #include <uORB/topics/vehicle_command.h> #include <uORB/topics/vehicle_local_position_setpoint.h> +#include <uORB/topics/vehicle_global_velocity_setpoint.h> #include <uORB/topics/position_setpoint_triplet.h> #include <uORB/topics/vehicle_vicon_position.h> #include <uORB/topics/vehicle_attitude_setpoint.h> @@ -102,8 +105,6 @@ public: static void *start_helper(void *context); private: - perf_counter_t _loop_perf; /**< loop performance counter */ - Mavlink *_mavlink; void handle_message(mavlink_message_t *msg); @@ -124,6 +125,7 @@ private: mavlink_status_t status; struct vehicle_local_position_s hil_local_pos; + struct vehicle_control_mode_s _control_mode; orb_advert_t _global_pos_pub; orb_advert_t _local_pos_pub; orb_advert_t _attitude_pub; @@ -138,15 +140,23 @@ private: orb_advert_t _cmd_pub; orb_advert_t _flow_pub; orb_advert_t _offboard_control_sp_pub; + orb_advert_t _local_pos_sp_pub; + orb_advert_t _global_vel_sp_pub; + orb_advert_t _att_sp_pub; + orb_advert_t _rates_sp_pub; orb_advert_t _vicon_position_pub; orb_advert_t _telemetry_status_pub; orb_advert_t _rc_pub; orb_advert_t _manual_pub; - hrt_abstime _telemetry_heartbeat_time; bool _radio_status_available; + int _control_mode_sub; int _hil_frames; uint64_t _old_timestamp; bool _hil_local_proj_inited; float _hil_local_alt0; struct map_projection_reference_s _hil_local_proj_ref; + + /* do not allow copying this class */ + MavlinkReceiver(const MavlinkReceiver&); + MavlinkReceiver operator=(const MavlinkReceiver&); }; diff --git a/src/modules/mavlink/mavlink_stream.cpp b/src/modules/mavlink/mavlink_stream.cpp index 7279206db..5b9e45d3e 100644 --- a/src/modules/mavlink/mavlink_stream.cpp +++ b/src/modules/mavlink/mavlink_stream.cpp @@ -43,9 +43,9 @@ #include "mavlink_stream.h" #include "mavlink_main.h" -MavlinkStream::MavlinkStream() : +MavlinkStream::MavlinkStream(Mavlink *mavlink) : next(nullptr), - _channel(MAVLINK_COMM_0), + _mavlink(mavlink), _interval(1000000), _last_sent(0) { @@ -65,26 +65,27 @@ MavlinkStream::set_interval(const unsigned int interval) } /** - * Set mavlink channel - */ -void -MavlinkStream::set_channel(mavlink_channel_t channel) -{ - _channel = channel; -} - -/** * Update subscriptions and send message if necessary */ int MavlinkStream::update(const hrt_abstime t) { uint64_t dt = t - _last_sent; + unsigned int interval = _interval; + + if (!const_rate()) { + interval /= _mavlink->get_rate_mult(); + } - if (dt > 0 && dt >= _interval) { + if (dt > 0 && dt >= interval) { /* interval expired, send message */ send(t); - _last_sent = (t / _interval) * _interval; + if (const_rate()) { + _last_sent = (t / _interval) * _interval; + + } else { + _last_sent = t; + } return 0; } diff --git a/src/modules/mavlink/mavlink_stream.h b/src/modules/mavlink/mavlink_stream.h index 69809a386..ef22dc443 100644 --- a/src/modules/mavlink/mavlink_stream.h +++ b/src/modules/mavlink/mavlink_stream.h @@ -46,37 +46,60 @@ class Mavlink; class MavlinkStream; -#include "mavlink_main.h" - class MavlinkStream { public: MavlinkStream *next; - MavlinkStream(); + MavlinkStream(Mavlink *mavlink); virtual ~MavlinkStream(); + + /** + * Get the interval + * + * @param interval the inveral in microseconds (us) between messages + */ void set_interval(const unsigned int interval); - void set_channel(mavlink_channel_t channel); + + /** + * Get the interval + * + * @return the inveral in microseconds (us) between messages + */ + unsigned get_interval() { return _interval; } /** * @return 0 if updated / sent, -1 if unchanged */ int update(const hrt_abstime t); - static MavlinkStream *new_instance(); + static MavlinkStream *new_instance(const Mavlink *mavlink); static const char *get_name_static(); - virtual void subscribe(Mavlink *mavlink) = 0; virtual const char *get_name() const = 0; virtual uint8_t get_id() = 0; + /** + * @return true if steam rate shouldn't be adjusted + */ + virtual bool const_rate() { return false; } + + /** + * Get maximal total messages size on update + */ + virtual unsigned get_size() = 0; + protected: - mavlink_channel_t _channel; + Mavlink * _mavlink; unsigned int _interval; virtual void send(const hrt_abstime t) = 0; private: hrt_abstime _last_sent; + + /* do not allow top copying this class */ + MavlinkStream(const MavlinkStream&); + MavlinkStream& operator=(const MavlinkStream&); }; diff --git a/src/modules/mavlink/module.mk b/src/modules/mavlink/module.mk index a4d8bfbfb..91fdd6154 100644 --- a/src/modules/mavlink/module.mk +++ b/src/modules/mavlink/module.mk @@ -39,11 +39,12 @@ MODULE_COMMAND = mavlink SRCS += mavlink_main.cpp \ mavlink.c \ mavlink_receiver.cpp \ + mavlink_mission.cpp \ + mavlink_parameters.cpp \ mavlink_orb_subscription.cpp \ mavlink_messages.cpp \ mavlink_stream.cpp \ mavlink_rate_limiter.cpp \ - mavlink_commands.cpp \ mavlink_ftp.cpp INCLUDE_DIRS += $(MAVLINK_SRC)/include/mavlink @@ -51,3 +52,5 @@ INCLUDE_DIRS += $(MAVLINK_SRC)/include/mavlink MAXOPTIMIZATION = -Os MODULE_STACKSIZE = 1024 + +EXTRACXXFLAGS = -Weffc++ diff --git a/src/modules/mavlink/waypoints.h b/src/modules/mavlink/waypoints.h deleted file mode 100644 index 532eff7aa..000000000 --- a/src/modules/mavlink/waypoints.h +++ /dev/null @@ -1,111 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2009-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 waypoints.h - * MAVLink waypoint protocol definition (BSD-relicensed). - * - * @author Petri Tanskanen <petri.tanskanen@inf.ethz.ch> - * @author Lorenz Meier <lm@inf.ethz.ch> - * @author Thomas Gubler <thomasgubler@student.ethz.ch> - * @author Julian Oes <joes@student.ethz.ch> - */ - -#ifndef WAYPOINTS_H_ -#define WAYPOINTS_H_ - -/* This assumes you have the mavlink headers on your include path - or in the same folder as this source file */ - -#include <v1.0/mavlink_types.h> -#include "mavlink_bridge_header.h" -#include <stdbool.h> -#include <uORB/topics/mission.h> - -enum MAVLINK_WPM_STATES { - MAVLINK_WPM_STATE_IDLE = 0, - MAVLINK_WPM_STATE_SENDLIST, - MAVLINK_WPM_STATE_SENDLIST_SENDWPS, - MAVLINK_WPM_STATE_GETLIST, - MAVLINK_WPM_STATE_GETLIST_GETWPS, - MAVLINK_WPM_STATE_GETLIST_GOTALL, - MAVLINK_WPM_STATE_ENUM_END -}; - -enum MAVLINK_WPM_CODES { - MAVLINK_WPM_CODE_OK = 0, - MAVLINK_WPM_CODE_ERR_WAYPOINT_ACTION_NOT_SUPPORTED, - MAVLINK_WPM_CODE_ERR_WAYPOINT_FRAME_NOT_SUPPORTED, - MAVLINK_WPM_CODE_ERR_WAYPOINT_OUT_OF_BOUNDS, - MAVLINK_WPM_CODE_ERR_WAYPOINT_MAX_NUMBER_EXCEEDED, - MAVLINK_WPM_CODE_ENUM_END -}; - - -#define MAVLINK_WPM_MAX_WP_COUNT 255 -#define MAVLINK_WPM_PROTOCOL_TIMEOUT_DEFAULT 5000000 ///< Protocol communication timeout in useconds -#define MAVLINK_WPM_SETPOINT_DELAY_DEFAULT 1000000 ///< When to send a new setpoint -#define MAVLINK_WPM_PROTOCOL_DELAY_DEFAULT 40000 - - -struct mavlink_wpm_storage { - uint16_t size; - uint16_t max_size; - enum MAVLINK_WPM_STATES current_state; - int16_t current_wp_id; ///< Waypoint in current transmission - uint16_t current_count; - uint8_t current_partner_sysid; - uint8_t current_partner_compid; - uint64_t timestamp_lastaction; - uint64_t timestamp_last_send_setpoint; - uint32_t timeout; - int current_dataman_id; -}; - -typedef struct mavlink_wpm_storage mavlink_wpm_storage; - -int map_mavlink_mission_item_to_mission_item(const mavlink_mission_item_t *mavlink_mission_item, struct mission_item_s *mission_item); -int map_mission_item_to_mavlink_mission_item(const struct mission_item_s *mission_item, mavlink_mission_item_t *mavlink_mission_item); - - -void mavlink_wpm_init(mavlink_wpm_storage *state); -void mavlink_waypoint_eventloop(uint64_t now); -void mavlink_wpm_message_handler(const mavlink_message_t *msg); - -extern void mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1, - float param2, float param3, float param4, float param5_lat_x, - float param6_lon_y, float param7_alt_z, uint8_t frame, uint16_t command); - -static uint8_t missionlib_msg_buf[MAVLINK_MAX_PACKET_LEN]; - -#endif /* WAYPOINTS_H_ */ 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 bcc3e2ae7..ecc40428d 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -105,34 +105,38 @@ public: int start(); private: + const float alt_ctl_dz = 0.2f; - bool _task_should_exit; /**< if true, task should exit */ + bool _task_should_exit; /**< if true, task should exit */ int _control_task; /**< task handle for task */ int _mavlink_fd; /**< mavlink fd */ int _att_sub; /**< vehicle attitude subscription */ int _att_sp_sub; /**< vehicle attitude setpoint */ int _control_mode_sub; /**< vehicle control mode subscription */ - int _params_sub; /**< notification of parameter updates */ - int _manual_sub; /**< notification of manual control updates */ + int _params_sub; /**< notification of parameter updates */ + int _manual_sub; /**< notification of manual control updates */ int _arming_sub; /**< arming status of outputs */ int _local_pos_sub; /**< vehicle local position */ - int _pos_sp_triplet_sub; /**< position setpoint triplet */ + int _pos_sp_triplet_sub; /**< position setpoint triplet */ + int _local_pos_sp_sub; /**< offboard local position setpoint */ + int _global_vel_sp_sub; /**< offboard global velocity setpoint */ orb_advert_t _att_sp_pub; /**< attitude setpoint publication */ orb_advert_t _local_pos_sp_pub; /**< vehicle local position setpoint publication */ orb_advert_t _global_vel_sp_pub; /**< vehicle global velocity setpoint publication */ struct vehicle_attitude_s _att; /**< vehicle attitude */ - struct vehicle_attitude_setpoint_s _att_sp; /**< vehicle attitude setpoint */ - struct manual_control_setpoint_s _manual; /**< r/c channel data */ - struct vehicle_control_mode_s _control_mode; /**< vehicle control mode */ + struct vehicle_attitude_setpoint_s _att_sp; /**< vehicle attitude setpoint */ + struct manual_control_setpoint_s _manual; /**< r/c channel data */ + struct vehicle_control_mode_s _control_mode; /**< vehicle control mode */ struct actuator_armed_s _arming; /**< actuator arming status */ - struct vehicle_local_position_s _local_pos; /**< vehicle local position */ + struct vehicle_local_position_s _local_pos; /**< vehicle local position */ struct position_setpoint_triplet_s _pos_sp_triplet; /**< vehicle global position setpoint triplet */ struct vehicle_local_position_setpoint_s _local_pos_sp; /**< vehicle local position setpoint */ struct vehicle_global_velocity_setpoint_s _global_vel_sp; /**< vehicle global velocity setpoint */ + struct { param_t thr_min; param_t thr_max; @@ -181,6 +185,8 @@ private: math::Vector<3> _vel; math::Vector<3> _vel_sp; math::Vector<3> _vel_prev; /**< velocity on previous step */ + math::Vector<3> _vel_ff; + math::Vector<3> _sp_move_rate; /** * Update our local parameter cache. @@ -214,6 +220,16 @@ private: void reset_alt_sp(); /** + * Set position setpoint using manual control + */ + void control_manual(float dt); + + /** + * Set position setpoint using offboard control + */ + void control_offboard(float dt); + + /** * Select between barometric and global (AMSL) altitudes */ void select_alt(bool global); @@ -256,6 +272,7 @@ MulticopterPositionControl::MulticopterPositionControl() : _arming_sub(-1), _local_pos_sub(-1), _pos_sp_triplet_sub(-1), + _global_vel_sp_sub(-1), /* publications */ _att_sp_pub(-1), @@ -293,6 +310,8 @@ MulticopterPositionControl::MulticopterPositionControl() : _vel.zero(); _vel_sp.zero(); _vel_prev.zero(); + _vel_ff.zero(); + _sp_move_rate.zero(); _params_handles.thr_min = param_find("MPC_THR_MIN"); _params_handles.thr_max = param_find("MPC_THR_MAX"); @@ -388,9 +407,11 @@ MulticopterPositionControl::parameters_update(bool force) param_get(_params_handles.z_vel_max, &v); _params.vel_max(2) = v; param_get(_params_handles.xy_ff, &v); + v = math::constrain(v, 0.0f, 1.0f); _params.vel_ff(0) = v; _params.vel_ff(1) = v; param_get(_params_handles.z_ff, &v); + v = math::constrain(v, 0.0f, 1.0f); _params.vel_ff(2) = v; _params.sp_offs_max = _params.vel_max.edivide(_params.pos_p) * 2.0f; @@ -493,8 +514,11 @@ MulticopterPositionControl::reset_pos_sp() { if (_reset_pos_sp) { _reset_pos_sp = false; - _pos_sp(0) = _pos(0); - _pos_sp(1) = _pos(1); + /* shift position setpoint to make attitude setpoint continuous */ + _pos_sp(0) = _pos(0) + (_vel(0) - _att_sp.R_body[0][2] * _att_sp.thrust / _params.vel_p(0) + - _params.vel_ff(0) * _sp_move_rate(0)) / _params.pos_p(0); + _pos_sp(1) = _pos(1) + (_vel(1) - _att_sp.R_body[1][2] * _att_sp.thrust / _params.vel_p(1) + - _params.vel_ff(1) * _sp_move_rate(1)) / _params.pos_p(1); mavlink_log_info(_mavlink_fd, "[mpc] reset pos sp: %.2f, %.2f", (double)_pos_sp(0), (double)_pos_sp(1)); } } @@ -504,12 +528,126 @@ MulticopterPositionControl::reset_alt_sp() { if (_reset_alt_sp) { _reset_alt_sp = false; - _pos_sp(2) = _pos(2); + _pos_sp(2) = _pos(2) + (_vel(2) - _params.vel_ff(2) * _sp_move_rate(2)) / _params.pos_p(2); mavlink_log_info(_mavlink_fd, "[mpc] reset alt sp: %.2f", -(double)_pos_sp(2)); } } void +MulticopterPositionControl::control_manual(float dt) +{ + _sp_move_rate.zero(); + + if (_control_mode.flag_control_altitude_enabled) { + /* move altitude setpoint with throttle stick */ + _sp_move_rate(2) = -scale_control(_manual.z - 0.5f, 0.5f, alt_ctl_dz); + } + + if (_control_mode.flag_control_position_enabled) { + /* move position setpoint with roll/pitch stick */ + _sp_move_rate(0) = _manual.x; + _sp_move_rate(1) = _manual.y; + } + + /* limit setpoint move rate */ + float sp_move_norm = _sp_move_rate.length(); + + if (sp_move_norm > 1.0f) { + _sp_move_rate /= sp_move_norm; + } + + /* _sp_move_rate scaled to 0..1, scale it to max speed and rotate around yaw */ + math::Matrix<3, 3> R_yaw_sp; + R_yaw_sp.from_euler(0.0f, 0.0f, _att_sp.yaw_body); + _sp_move_rate = R_yaw_sp * _sp_move_rate.emult(_params.vel_max); + + if (_control_mode.flag_control_altitude_enabled) { + /* reset alt setpoint to current altitude if needed */ + reset_alt_sp(); + } + + if (_control_mode.flag_control_position_enabled) { + /* reset position setpoint to current position if needed */ + reset_pos_sp(); + } + + /* feed forward setpoint move rate with weight vel_ff */ + _vel_ff = _sp_move_rate.emult(_params.vel_ff); + + /* move position setpoint */ + _pos_sp += _sp_move_rate * dt; + + /* check if position setpoint is too far from actual position */ + math::Vector<3> pos_sp_offs; + pos_sp_offs.zero(); + + if (_control_mode.flag_control_position_enabled) { + pos_sp_offs(0) = (_pos_sp(0) - _pos(0)) / _params.sp_offs_max(0); + pos_sp_offs(1) = (_pos_sp(1) - _pos(1)) / _params.sp_offs_max(1); + } + + if (_control_mode.flag_control_altitude_enabled) { + pos_sp_offs(2) = (_pos_sp(2) - _pos(2)) / _params.sp_offs_max(2); + } + + float pos_sp_offs_norm = pos_sp_offs.length(); + + if (pos_sp_offs_norm > 1.0f) { + pos_sp_offs /= pos_sp_offs_norm; + _pos_sp = _pos + pos_sp_offs.emult(_params.sp_offs_max); + } +} + +void +MulticopterPositionControl::control_offboard(float dt) +{ + bool updated; + orb_check(_pos_sp_triplet_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_sub, &_pos_sp_triplet); + } + + if (_pos_sp_triplet.current.valid) { + if (_control_mode.flag_control_position_enabled && _pos_sp_triplet.current.position_valid) { + /* control position */ + _pos_sp(0) = _pos_sp_triplet.current.x; + _pos_sp(1) = _pos_sp_triplet.current.y; + _pos_sp(2) = _pos_sp_triplet.current.z; + _att_sp.yaw_body = _pos_sp_triplet.current.yaw; + + } else if (_control_mode.flag_control_velocity_enabled && _pos_sp_triplet.current.velocity_valid) { + /* control velocity */ + /* reset position setpoint to current position if needed */ + reset_pos_sp(); + + /* set position setpoint move rate */ + _sp_move_rate(0) = _pos_sp_triplet.current.vx; + _sp_move_rate(1) = _pos_sp_triplet.current.vy; + _att_sp.yaw_body = _att_sp.yaw_body + _pos_sp_triplet.current.yawspeed * dt; + } + + if (_control_mode.flag_control_altitude_enabled) { + /* reset alt setpoint to current altitude if needed */ + reset_alt_sp(); + + /* set altitude setpoint move rate */ + _sp_move_rate(2) = _pos_sp_triplet.current.vz; + } + + /* feed forward setpoint move rate with weight vel_ff */ + _vel_ff = _sp_move_rate.emult(_params.vel_ff); + + /* move position setpoint */ + _pos_sp += _sp_move_rate * dt; + + } else { + reset_pos_sp(); + reset_alt_sp(); + } +} + +void MulticopterPositionControl::task_main() { warnx("started"); @@ -528,6 +666,9 @@ MulticopterPositionControl::task_main() _arming_sub = orb_subscribe(ORB_ID(actuator_armed)); _local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position)); _pos_sp_triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet)); + _local_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_local_position_setpoint)); + _global_vel_sp_sub = orb_subscribe(ORB_ID(vehicle_global_velocity_setpoint)); + parameters_update(true); @@ -544,10 +685,6 @@ MulticopterPositionControl::task_main() hrt_abstime t_prev = 0; - const float alt_ctl_dz = 0.2f; - - math::Vector<3> sp_move_rate; - sp_move_rate.zero(); math::Vector<3> thrust_int; thrust_int.zero(); math::Matrix<3, 3> R; @@ -606,62 +743,17 @@ MulticopterPositionControl::task_main() _vel(1) = _local_pos.vy; _vel(2) = _local_pos.vz; - sp_move_rate.zero(); + _vel_ff.zero(); + _sp_move_rate.zero(); /* select control source */ if (_control_mode.flag_control_manual_enabled) { /* manual control */ - if (_control_mode.flag_control_altitude_enabled) { - /* reset alt setpoint to current altitude if needed */ - reset_alt_sp(); - - /* move altitude setpoint with throttle stick */ - sp_move_rate(2) = -scale_control(_manual.z - 0.5f, 0.5f, alt_ctl_dz); - } + control_manual(dt); - if (_control_mode.flag_control_position_enabled) { - /* reset position setpoint to current position if needed */ - reset_pos_sp(); - - /* move position setpoint with roll/pitch stick */ - sp_move_rate(0) = _manual.x; - sp_move_rate(1) = _manual.y; - } - - /* limit setpoint move rate */ - float sp_move_norm = sp_move_rate.length(); - - if (sp_move_norm > 1.0f) { - sp_move_rate /= sp_move_norm; - } - - /* scale to max speed and rotate around yaw */ - math::Matrix<3, 3> R_yaw_sp; - R_yaw_sp.from_euler(0.0f, 0.0f, _att_sp.yaw_body); - sp_move_rate = R_yaw_sp * sp_move_rate.emult(_params.vel_max); - - /* move position setpoint */ - _pos_sp += sp_move_rate * dt; - - /* check if position setpoint is too far from actual position */ - math::Vector<3> pos_sp_offs; - pos_sp_offs.zero(); - - if (_control_mode.flag_control_position_enabled) { - pos_sp_offs(0) = (_pos_sp(0) - _pos(0)) / _params.sp_offs_max(0); - pos_sp_offs(1) = (_pos_sp(1) - _pos(1)) / _params.sp_offs_max(1); - } - - if (_control_mode.flag_control_altitude_enabled) { - pos_sp_offs(2) = (_pos_sp(2) - _pos(2)) / _params.sp_offs_max(2); - } - - float pos_sp_offs_norm = pos_sp_offs.length(); - - if (pos_sp_offs_norm > 1.0f) { - pos_sp_offs /= pos_sp_offs_norm; - _pos_sp = _pos + pos_sp_offs.emult(_params.sp_offs_max); - } + } else if (_control_mode.flag_control_offboard_enabled) { + /* offboard control */ + control_offboard(dt); } else { /* AUTO */ @@ -696,6 +788,7 @@ MulticopterPositionControl::task_main() } /* fill local position setpoint */ + _local_pos_sp.timestamp = hrt_absolute_time(); _local_pos_sp.x = _pos_sp(0); _local_pos_sp.y = _pos_sp(1); _local_pos_sp.z = _pos_sp(2); @@ -709,6 +802,7 @@ MulticopterPositionControl::task_main() _local_pos_sp_pub = orb_advertise(ORB_ID(vehicle_local_position_setpoint), &_local_pos_sp); } + if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid && _pos_sp_triplet.current.type == SETPOINT_TYPE_IDLE) { /* idle state, don't run controller and set zero thrust */ R.identity(); @@ -734,7 +828,7 @@ MulticopterPositionControl::task_main() /* run position & altitude controllers, calculate velocity setpoint */ math::Vector<3> pos_err = _pos_sp - _pos; - _vel_sp = pos_err.emult(_params.pos_p) + sp_move_rate.emult(_params.vel_ff); + _vel_sp = pos_err.emult(_params.pos_p) + _vel_ff; if (!_control_mode.flag_control_altitude_enabled) { _reset_alt_sp = true; @@ -752,6 +846,7 @@ MulticopterPositionControl::task_main() _vel_sp(2) = _params.land_speed; } + if (!_control_mode.flag_control_manual_enabled) { /* limit 3D speed only in non-manual modes */ float vel_sp_norm = _vel_sp.edivide(_params.vel_max).length(); @@ -813,7 +908,7 @@ MulticopterPositionControl::task_main() math::Vector<3> vel_err = _vel_sp - _vel; /* derivative of velocity error, not includes setpoint acceleration */ - math::Vector<3> vel_err_d = (sp_move_rate - _vel).emult(_params.pos_p) - (_vel - _vel_prev) / dt; + math::Vector<3> vel_err_d = (_sp_move_rate - _vel).emult(_params.pos_p) - (_vel - _vel_prev) / dt; _vel_prev = _vel; /* thrust vector in NED frame */ @@ -1039,6 +1134,7 @@ MulticopterPositionControl::task_main() _reset_pos_sp = true; reset_int_z = true; reset_int_xy = true; + } /* reset altitude controller integral (hovering throttle) to manual throttle after manual throttle control */ diff --git a/src/modules/navigator/loiter.cpp b/src/modules/navigator/loiter.cpp index 542483fb1..f827e70c9 100644 --- a/src/modules/navigator/loiter.cpp +++ b/src/modules/navigator/loiter.cpp @@ -36,6 +36,7 @@ * Helper class to loiter * * @author Julian Oes <julian@oes.ch> + * @author Anton Babushkin <anton.babushkin@me.com> */ #include <string.h> @@ -51,28 +52,42 @@ #include <uORB/topics/position_setpoint_triplet.h> #include "loiter.h" +#include "navigator.h" Loiter::Loiter(Navigator *navigator, const char *name) : MissionBlock(navigator, name) { /* load initial params */ updateParams(); - /* initial reset */ - on_inactive(); } Loiter::~Loiter() { } -bool -Loiter::on_active(struct position_setpoint_triplet_s *pos_sp_triplet) +void +Loiter::on_inactive() { - /* set loiter item, don't reuse an existing position setpoint */ - return set_loiter_item(pos_sp_triplet); } void -Loiter::on_inactive() +Loiter::on_activation() +{ + /* set current mission item to loiter */ + set_loiter_item(&_mission_item); + + /* convert mission item to current setpoint */ + struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); + pos_sp_triplet->previous.valid = false; + mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current); + pos_sp_triplet->next.valid = false; + + _navigator->set_can_loiter_at_sp(pos_sp_triplet->current.type == SETPOINT_TYPE_LOITER); + + _navigator->set_position_setpoint_triplet_updated(); +} + +void +Loiter::on_active() { } diff --git a/src/modules/navigator/loiter.h b/src/modules/navigator/loiter.h index 65ff5c31e..37ab57a07 100644 --- a/src/modules/navigator/loiter.h +++ b/src/modules/navigator/loiter.h @@ -50,25 +50,15 @@ class Loiter : public MissionBlock { public: - /** - * Constructor - */ Loiter(Navigator *navigator, const char *name); - /** - * Destructor - */ ~Loiter(); - /** - * This function is called while the mode is inactive - */ virtual void on_inactive(); - /** - * This function is called while the mode is active - */ - virtual bool on_active(struct position_setpoint_triplet_s *pos_sp_triplet); + virtual void on_activation(); + + virtual void on_active(); }; #endif diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 72255103b..c0e37a3ed 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -54,25 +54,28 @@ #include <uORB/topics/mission.h> #include <uORB/topics/mission_result.h> -#include "navigator.h" #include "mission.h" +#include "navigator.h" Mission::Mission(Navigator *navigator, const char *name) : MissionBlock(navigator, name), _param_onboard_enabled(this, "ONBOARD_EN"), + _param_takeoff_alt(this, "TAKEOFF_ALT"), + _param_dist_1wp(this, "DIST_1WP"), _onboard_mission({0}), _offboard_mission({0}), _current_onboard_mission_index(-1), _current_offboard_mission_index(-1), + _need_takeoff(true), + _takeoff(false), _mission_result_pub(-1), _mission_result({0}), - _mission_type(MISSION_TYPE_NONE) + _mission_type(MISSION_TYPE_NONE), + _inited(false), + _dist_1wp_ok(false) { /* load initial params */ updateParams(); - /* set initial mission items */ - on_inactive(); - } Mission::~Mission() @@ -82,55 +85,71 @@ Mission::~Mission() void Mission::on_inactive() { - _first_run = true; + if (_inited) { + /* check if missions have changed so that feedback to ground station is given */ + bool onboard_updated = false; + orb_check(_navigator->get_onboard_mission_sub(), &onboard_updated); + if (onboard_updated) { + update_onboard_mission(); + } + + bool offboard_updated = false; + orb_check(_navigator->get_offboard_mission_sub(), &offboard_updated); + if (offboard_updated) { + update_offboard_mission(); + } + + } else { + /* read mission topics on initialization */ + _inited = true; - /* check anyway if missions have changed so that feedback to groundstation is given */ - bool onboard_updated; - orb_check(_navigator->get_onboard_mission_sub(), &onboard_updated); - if (onboard_updated) { update_onboard_mission(); + update_offboard_mission(); } - bool offboard_updated; - orb_check(_navigator->get_offboard_mission_sub(), &offboard_updated); - if (offboard_updated) { - update_offboard_mission(); + if (!_navigator->get_can_loiter_at_sp() || _navigator->get_vstatus()->condition_landed) { + _need_takeoff = true; } } -bool -Mission::on_active(struct position_setpoint_triplet_s *pos_sp_triplet) +void +Mission::on_activation() { - bool updated = false; + set_mission_items(); +} +void +Mission::on_active() +{ /* check if anything has changed */ - bool onboard_updated; + bool onboard_updated = false; orb_check(_navigator->get_onboard_mission_sub(), &onboard_updated); if (onboard_updated) { update_onboard_mission(); } - bool offboard_updated; + bool offboard_updated = false; orb_check(_navigator->get_offboard_mission_sub(), &offboard_updated); if (offboard_updated) { update_offboard_mission(); } /* reset mission items if needed */ - if (onboard_updated || offboard_updated || _first_run) { - set_mission_items(pos_sp_triplet); - updated = true; - _first_run = false; + if (onboard_updated || offboard_updated) { + set_mission_items(); } /* lets check if we reached the current mission item */ if (_mission_type != MISSION_TYPE_NONE && is_mission_item_reached()) { advance_mission(); - set_mission_items(pos_sp_triplet); - updated = true; - } + set_mission_items(); - return updated; + } else { + /* if waypoint position reached allow loiter on the setpoint */ + if (_waypoint_position_reached && _mission_item.nav_cmd != NAV_CMD_IDLE) { + _navigator->set_can_loiter_at_sp(true); + } + } } void @@ -138,9 +157,9 @@ Mission::update_onboard_mission() { if (orb_copy(ORB_ID(onboard_mission), _navigator->get_onboard_mission_sub(), &_onboard_mission) == OK) { /* accept the current index set by the onboard mission if it is within bounds */ - if (_onboard_mission.current_index >=0 - && _onboard_mission.current_index < (int)_onboard_mission.count) { - _current_onboard_mission_index = _onboard_mission.current_index; + if (_onboard_mission.current_seq >=0 + && _onboard_mission.current_seq < (int)_onboard_mission.count) { + _current_onboard_mission_index = _onboard_mission.current_seq; } else { /* if less WPs available, reset to first WP */ if (_current_onboard_mission_index >= (int)_onboard_mission.count) { @@ -151,9 +170,10 @@ Mission::update_onboard_mission() } /* otherwise, just leave it */ } + } else { _onboard_mission.count = 0; - _onboard_mission.current_index = 0; + _onboard_mission.current_seq = 0; _current_onboard_mission_index = 0; } } @@ -162,15 +182,15 @@ void Mission::update_offboard_mission() { if (orb_copy(ORB_ID(offboard_mission), _navigator->get_offboard_mission_sub(), &_offboard_mission) == OK) { - + warnx("offboard mission updated: dataman_id=%d, count=%d, current_seq=%d", _offboard_mission.dataman_id, _offboard_mission.count, _offboard_mission.current_seq); /* determine current index */ - if (_offboard_mission.current_index >= 0 - && _offboard_mission.current_index < (int)_offboard_mission.count) { - _current_offboard_mission_index = _offboard_mission.current_index; + if (_offboard_mission.current_seq >= 0 && _offboard_mission.current_seq < (int)_offboard_mission.count) { + _current_offboard_mission_index = _offboard_mission.current_seq; } else { - /* if less WPs available, reset to first WP */ + /* if less items available, reset to first item */ if (_current_offboard_mission_index >= (int)_offboard_mission.count) { _current_offboard_mission_index = 0; + /* if not initialized, set it to 0 */ } else if (_current_offboard_mission_index < 0) { _current_offboard_mission_index = 0; @@ -180,23 +200,19 @@ Mission::update_offboard_mission() /* Check mission feasibility, for now do not handle the return value, * however warnings are issued to the gcs via mavlink from inside the MissionFeasiblityChecker */ - dm_item_t dm_current; + dm_item_t dm_current = DM_KEY_WAYPOINTS_OFFBOARD(_offboard_mission.dataman_id); - if (_offboard_mission.dataman_id == 0) { - dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0; - } else { - dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1; - } + missionFeasiblityChecker.checkMissionFeasible(_navigator->get_vstatus()->is_rotary_wing, + dm_current, (size_t) _offboard_mission.count, _navigator->get_geofence(), + _navigator->get_home_position()->alt); - missionFeasiblityChecker.checkMissionFeasible(_navigator->get_vstatus()->is_rotary_wing, dm_current, - (size_t)_offboard_mission.count, - _navigator->get_geofence(), - _navigator->get_home_position()->alt); } else { + warnx("offboard mission update failed"); _offboard_mission.count = 0; - _offboard_mission.current_index = 0; + _offboard_mission.current_seq = 0; _current_offboard_mission_index = 0; } + report_current_offboard_mission_item(); } @@ -204,227 +220,375 @@ Mission::update_offboard_mission() void Mission::advance_mission() { - switch (_mission_type) { - case MISSION_TYPE_ONBOARD: - _current_onboard_mission_index++; - break; - - case MISSION_TYPE_OFFBOARD: - _current_offboard_mission_index++; - break; - - case MISSION_TYPE_NONE: - default: - break; + if (_takeoff) { + _takeoff = false; + + } else { + switch (_mission_type) { + case MISSION_TYPE_ONBOARD: + _current_onboard_mission_index++; + break; + + case MISSION_TYPE_OFFBOARD: + _current_offboard_mission_index++; + break; + + case MISSION_TYPE_NONE: + default: + break; + } + } +} + +bool +Mission::check_dist_1wp() +{ + if (_dist_1wp_ok) { + /* always return true after at least one successful check */ + return true; + } + + /* check if first waypoint is not too far from home */ + if (_param_dist_1wp.get() > 0.0f) { + if (_navigator->get_vstatus()->condition_home_position_valid) { + struct mission_item_s mission_item; + + /* find first waypoint (with lat/lon) item in datamanager */ + for (unsigned i = 0; i < _offboard_mission.count; i++) { + if (dm_read(DM_KEY_WAYPOINTS_OFFBOARD(_offboard_mission.dataman_id), i, + &mission_item, sizeof(mission_item_s)) == sizeof(mission_item_s)) { + + /* check only items with valid lat/lon */ + if ( mission_item.nav_cmd == NAV_CMD_WAYPOINT || + mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT || + mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT || + mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED || + mission_item.nav_cmd == NAV_CMD_TAKEOFF || + mission_item.nav_cmd == NAV_CMD_PATHPLANNING) { + + /* check distance from home to item */ + float dist_to_1wp = get_distance_to_next_waypoint( + mission_item.lat, mission_item.lon, + _navigator->get_home_position()->lat, _navigator->get_home_position()->lon); + + if (dist_to_1wp < _param_dist_1wp.get()) { + _dist_1wp_ok = true; + if (dist_to_1wp > ((_param_dist_1wp.get() * 3) / 2)) { + /* allow at 2/3 distance, but warn */ + mavlink_log_critical(_navigator->get_mavlink_fd(), "Warning: First waypoint very far: %d m", (int)dist_to_1wp); + } + return true; + + } else { + /* item is too far from home */ + mavlink_log_critical(_navigator->get_mavlink_fd(), "Waypoint too far: %d m,[MIS_DIST_1WP=%d]", (int)dist_to_1wp, (int)_param_dist_1wp.get()); + return false; + } + } + + } else { + /* error reading, mission is invalid */ + mavlink_log_info(_navigator->get_mavlink_fd(), "error reading offboard mission"); + return false; + } + } + + /* no waypoints found in mission, then we will not fly far away */ + _dist_1wp_ok = true; + return true; + + } else { + mavlink_log_info(_navigator->get_mavlink_fd(), "no home position"); + return false; + } + + } else { + return true; } } void -Mission::set_mission_items(struct position_setpoint_triplet_s *pos_sp_triplet) +Mission::set_mission_items() { - set_previous_pos_setpoint(pos_sp_triplet); + /* make sure param is up to date */ + updateParams(); + + struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); + + /* set previous position setpoint to current */ + set_previous_pos_setpoint(); + + /* get home distance state */ + bool home_dist_ok = check_dist_1wp(); + /* the home dist check provides user feedback, so we initialize it to this */ + bool user_feedback_done = !home_dist_ok; /* try setting onboard mission item */ - if (is_current_onboard_mission_item_set(&pos_sp_triplet->current)) { + if (_param_onboard_enabled.get() && read_mission_item(true, true, &_mission_item)) { /* if mission type changed, notify */ if (_mission_type != MISSION_TYPE_ONBOARD) { - mavlink_log_info(_navigator->get_mavlink_fd(), - "#audio: onboard mission running"); + mavlink_log_critical(_navigator->get_mavlink_fd(), "onboard mission now running"); } _mission_type = MISSION_TYPE_ONBOARD; - _navigator->set_can_loiter_at_sp(false); /* try setting offboard mission item */ - } else if (is_current_offboard_mission_item_set(&pos_sp_triplet->current)) { + } else if (home_dist_ok && read_mission_item(false, true, &_mission_item)) { /* if mission type changed, notify */ if (_mission_type != MISSION_TYPE_OFFBOARD) { - mavlink_log_info(_navigator->get_mavlink_fd(), - "#audio: offboard mission running"); + mavlink_log_critical(_navigator->get_mavlink_fd(), "offboard mission now running"); } _mission_type = MISSION_TYPE_OFFBOARD; - _navigator->set_can_loiter_at_sp(false); + } else { + /* no mission available or mission finished, switch to loiter */ if (_mission_type != MISSION_TYPE_NONE) { - mavlink_log_info(_navigator->get_mavlink_fd(), - "#audio: mission finished"); - } else { - mavlink_log_info(_navigator->get_mavlink_fd(), - "#audio: no mission available"); + mavlink_log_critical(_navigator->get_mavlink_fd(), "mission finished, loitering"); + } else if (!user_feedback_done) { + /* only tell users that we got no mission if there has not been any + * better, more specific feedback yet + */ + mavlink_log_critical(_navigator->get_mavlink_fd(), "no valid mission available"); } _mission_type = MISSION_TYPE_NONE; - _navigator->set_can_loiter_at_sp(pos_sp_triplet->current.valid && _waypoint_position_reached); - set_loiter_item(pos_sp_triplet); + /* set loiter mission item */ + set_loiter_item(&_mission_item); + + /* update position setpoint triplet */ + pos_sp_triplet->previous.valid = false; + mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current); + pos_sp_triplet->next.valid = false; + + _navigator->set_can_loiter_at_sp(pos_sp_triplet->current.type == SETPOINT_TYPE_LOITER); + reset_mission_item_reached(); report_mission_finished(); + + _navigator->set_position_setpoint_triplet_updated(); + return; } -} -bool -Mission::is_current_onboard_mission_item_set(struct position_setpoint_s *current_pos_sp) -{ - /* make sure param is up to date */ - updateParams(); - if (_param_onboard_enabled.get() > 0 && - _current_onboard_mission_index >= 0&& - _current_onboard_mission_index < (int)_onboard_mission.count) { - struct mission_item_s new_mission_item; - if (read_mission_item(DM_KEY_WAYPOINTS_ONBOARD, true, &_current_onboard_mission_index, - &new_mission_item)) { - /* convert the current mission item and set it valid */ - mission_item_to_position_setpoint(&new_mission_item, current_pos_sp); - current_pos_sp->valid = true; - - reset_mission_item_reached(); - - /* TODO: report this somehow */ - memcpy(&_mission_item, &new_mission_item, sizeof(struct mission_item_s)); - return true; + /* do takeoff on first waypoint for rotary wing vehicles */ + if (_navigator->get_vstatus()->is_rotary_wing) { + /* force takeoff if landed (additional protection) */ + if (!_takeoff && _navigator->get_vstatus()->condition_landed) { + _need_takeoff = true; + } + + /* new current mission item set, check if we need takeoff */ + if (_need_takeoff && ( + _mission_item.nav_cmd == NAV_CMD_TAKEOFF || + _mission_item.nav_cmd == NAV_CMD_WAYPOINT || + _mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT || + _mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT || + _mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED || + _mission_item.nav_cmd == NAV_CMD_RETURN_TO_LAUNCH)) { + _takeoff = true; + _need_takeoff = false; } } - return false; -} -bool -Mission::is_current_offboard_mission_item_set(struct position_setpoint_s *current_pos_sp) -{ - if (_current_offboard_mission_index >= 0 && - _current_offboard_mission_index < (int)_offboard_mission.count) { - dm_item_t dm_current; - if (_offboard_mission.dataman_id == 0) { - dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0; + if (_takeoff) { + /* do takeoff before going to setpoint */ + /* set mission item as next position setpoint */ + mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->next); + + /* calculate takeoff altitude */ + float takeoff_alt = _mission_item.altitude; + if (_mission_item.altitude_is_relative) { + takeoff_alt += _navigator->get_home_position()->alt; + } + + /* perform takeoff at least to NAV_TAKEOFF_ALT above home/ground, even if first waypoint is lower */ + if (_navigator->get_vstatus()->condition_landed) { + takeoff_alt = fmaxf(takeoff_alt, _navigator->get_global_position()->alt + _param_takeoff_alt.get()); + } else { - dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1; + takeoff_alt = fmaxf(takeoff_alt, _navigator->get_home_position()->alt + _param_takeoff_alt.get()); } - struct mission_item_s new_mission_item; - if (read_mission_item(dm_current, true, &_current_offboard_mission_index, &new_mission_item)) { - /* convert the current mission item and set it valid */ - mission_item_to_position_setpoint(&new_mission_item, current_pos_sp); - current_pos_sp->valid = true; - reset_mission_item_reached(); + mavlink_log_critical(_navigator->get_mavlink_fd(), "takeoff to %.1f meters above home", (double)(takeoff_alt - _navigator->get_home_position()->alt)); - report_current_offboard_mission_item(); - memcpy(&_mission_item, &new_mission_item, sizeof(struct mission_item_s)); - return true; + _mission_item.lat = _navigator->get_global_position()->lat; + _mission_item.lon = _navigator->get_global_position()->lon; + _mission_item.altitude = takeoff_alt; + _mission_item.altitude_is_relative = false; + + mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current); + + } else { + /* set current position setpoint from mission item */ + mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current); + + /* require takeoff after landing or idle */ + if (pos_sp_triplet->current.type == SETPOINT_TYPE_LAND || pos_sp_triplet->current.type == SETPOINT_TYPE_IDLE) { + _need_takeoff = true; } - } - return false; -} -void -Mission::get_next_onboard_mission_item(struct position_setpoint_s *next_pos_sp) -{ - int next_temp_mission_index = _onboard_mission.current_index + 1; - - /* try if there is a next onboard mission */ - if (_onboard_mission.current_index >= 0 && - next_temp_mission_index < (int)_onboard_mission.count) { - struct mission_item_s new_mission_item; - if (read_mission_item(DM_KEY_WAYPOINTS_ONBOARD, false, &next_temp_mission_index, &new_mission_item)) { - /* convert next mission item to position setpoint */ - mission_item_to_position_setpoint(&new_mission_item, next_pos_sp); - next_pos_sp->valid = true; - return; + _navigator->set_can_loiter_at_sp(false); + reset_mission_item_reached(); + + if (_mission_type == MISSION_TYPE_OFFBOARD) { + report_current_offboard_mission_item(); } - } + // TODO: report onboard mission item somehow - /* give up */ - next_pos_sp->valid = false; - return; -} + /* try to read next mission item */ + struct mission_item_s mission_item_next; + + if (read_mission_item(_mission_type == MISSION_TYPE_ONBOARD, false, &mission_item_next)) { + /* got next mission item, update setpoint triplet */ + mission_item_to_position_setpoint(&mission_item_next, &pos_sp_triplet->next); -void -Mission::get_next_offboard_mission_item(struct position_setpoint_s *next_pos_sp) -{ - /* try if there is a next offboard mission */ - int next_temp_mission_index = _offboard_mission.current_index + 1; - warnx("next index: %d, count; %d", next_temp_mission_index, _offboard_mission.count); - if (_offboard_mission.current_index >= 0 && - next_temp_mission_index < (int)_offboard_mission.count) { - dm_item_t dm_current; - if (_offboard_mission.dataman_id == 0) { - dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0; } else { - dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1; - } - struct mission_item_s new_mission_item; - if (read_mission_item(dm_current, false, &next_temp_mission_index, &new_mission_item)) { - /* convert next mission item to position setpoint */ - mission_item_to_position_setpoint(&new_mission_item, next_pos_sp); - next_pos_sp->valid = true; - return; + /* next mission item is not available */ + pos_sp_triplet->next.valid = false; } } - /* give up */ - next_pos_sp->valid = false; - return; + + _navigator->set_position_setpoint_triplet_updated(); } bool -Mission::read_mission_item(const dm_item_t dm_item, bool is_current, int *mission_index, - struct mission_item_s *new_mission_item) +Mission::read_mission_item(bool onboard, bool is_current, struct mission_item_s *mission_item) { + /* select onboard/offboard mission */ + int *mission_index_ptr; + struct mission_s *mission; + dm_item_t dm_item; + int mission_index_next; + + if (onboard) { + /* onboard mission */ + mission_index_next = _current_onboard_mission_index + 1; + mission_index_ptr = is_current ? &_current_onboard_mission_index : &mission_index_next; + + mission = &_onboard_mission; + + dm_item = DM_KEY_WAYPOINTS_ONBOARD; + + } else { + /* offboard mission */ + mission_index_next = _current_offboard_mission_index + 1; + mission_index_ptr = is_current ? &_current_offboard_mission_index : &mission_index_next; + + mission = &_offboard_mission; + + dm_item = DM_KEY_WAYPOINTS_OFFBOARD(_offboard_mission.dataman_id); + } + + if (*mission_index_ptr < 0 || *mission_index_ptr >= (int)mission->count) { + /* mission item index out of bounds */ + return false; + } + /* repeat several to get the mission item because we might have to follow multiple DO_JUMPS */ - for (int i=0; i<10; i++) { + for (int i = 0; i < 10; i++) { const ssize_t len = sizeof(struct mission_item_s); + /* read mission item to temp storage first to not overwrite current mission item if data damaged */ + struct mission_item_s mission_item_tmp; + /* read mission item from datamanager */ - if (dm_read(dm_item, *mission_index, new_mission_item, len) != len) { + if (dm_read(dm_item, *mission_index_ptr, &mission_item_tmp, len) != len) { /* not supposed to happen unless the datamanager can't access the SD card, etc. */ mavlink_log_critical(_navigator->get_mavlink_fd(), - "#audio: ERROR waypoint could not be read"); + "ERROR waypoint could not be read"); return false; } /* check for DO_JUMP item, and whether it hasn't not already been repeated enough times */ - if (new_mission_item->nav_cmd == NAV_CMD_DO_JUMP) { + if (mission_item_tmp.nav_cmd == NAV_CMD_DO_JUMP) { /* do DO_JUMP as many times as requested */ - if (new_mission_item->do_jump_current_count < new_mission_item->do_jump_repeat_count) { + if (mission_item_tmp.do_jump_current_count < mission_item_tmp.do_jump_repeat_count) { /* only raise the repeat count if this is for the current mission item * but not for the next mission item */ if (is_current) { - (new_mission_item->do_jump_current_count)++; + (mission_item_tmp.do_jump_current_count)++; /* save repeat count */ - if (dm_write(dm_item, *mission_index, DM_PERSIST_IN_FLIGHT_RESET, - new_mission_item, len) != len) { + if (dm_write(dm_item, *mission_index_ptr, DM_PERSIST_IN_FLIGHT_RESET, &mission_item_tmp, len) != len) { /* not supposed to happen unless the datamanager can't access the * dataman */ mavlink_log_critical(_navigator->get_mavlink_fd(), - "#audio: ERROR DO JUMP waypoint could not be written"); + "ERROR DO JUMP waypoint could not be written"); return false; } } /* set new mission item index and repeat * we don't have to validate here, if it's invalid, we should realize this later .*/ - *mission_index = new_mission_item->do_jump_mission_index; + *mission_index_ptr = mission_item_tmp.do_jump_mission_index; + } else { - mavlink_log_info(_navigator->get_mavlink_fd(), - "#audio: DO JUMP repetitions completed"); + mavlink_log_critical(_navigator->get_mavlink_fd(), + "DO JUMP repetitions completed"); /* no more DO_JUMPS, therefore just try to continue with next mission item */ - (*mission_index)++; + (*mission_index_ptr)++; } } else { /* if it's not a DO_JUMP, then we were successful */ + memcpy(mission_item, &mission_item_tmp, sizeof(struct mission_item_s)); return true; } } /* we have given up, we don't want to cycle forever */ mavlink_log_critical(_navigator->get_mavlink_fd(), - "#audio: ERROR DO JUMP is cycling, giving up"); + "ERROR DO JUMP is cycling, giving up"); return false; } void +Mission::save_offboard_mission_state() +{ + mission_s mission_state; + + /* lock MISSION_STATE item */ + dm_lock(DM_KEY_MISSION_STATE); + + /* read current state */ + int read_res = dm_read(DM_KEY_MISSION_STATE, 0, &mission_state, sizeof(mission_s)); + + if (read_res == sizeof(mission_s)) { + /* data read successfully, check dataman ID and items count */ + if (mission_state.dataman_id == _offboard_mission.dataman_id && mission_state.count == _offboard_mission.count) { + /* navigator may modify only sequence, write modified state only if it changed */ + if (mission_state.current_seq != _current_offboard_mission_index) { + if (dm_write(DM_KEY_MISSION_STATE, 0, DM_PERSIST_POWER_ON_RESET, &mission_state, sizeof(mission_s)) != sizeof(mission_s)) { + warnx("ERROR: can't save mission state"); + mavlink_log_critical(_navigator->get_mavlink_fd(), "ERROR: can't save mission state"); + } + } + } + + } else { + /* invalid data, this must not happen and indicates error in offboard_mission publisher */ + mission_state.dataman_id = _offboard_mission.dataman_id; + mission_state.count = _offboard_mission.count; + mission_state.current_seq = _current_offboard_mission_index; + + warnx("ERROR: invalid mission state"); + mavlink_log_critical(_navigator->get_mavlink_fd(), "ERROR: invalid mission state"); + + /* write modified state only if changed */ + if (dm_write(DM_KEY_MISSION_STATE, 0, DM_PERSIST_POWER_ON_RESET, &mission_state, sizeof(mission_s)) != sizeof(mission_s)) { + warnx("ERROR: can't save mission state"); + mavlink_log_critical(_navigator->get_mavlink_fd(), "ERROR: can't save mission state"); + } + } + + /* unlock MISSION_STATE item */ + dm_unlock(DM_KEY_MISSION_STATE); +} + +void Mission::report_mission_item_reached() { if (_mission_type == MISSION_TYPE_OFFBOARD) { - _mission_result.mission_reached = true; - _mission_result.mission_index_reached = _current_offboard_mission_index; + _mission_result.reached = true; + _mission_result.seq_reached = _current_offboard_mission_index; } publish_mission_result(); } @@ -432,14 +596,17 @@ Mission::report_mission_item_reached() void Mission::report_current_offboard_mission_item() { - _mission_result.index_current_mission = _current_offboard_mission_index; + warnx("current offboard mission index: %d", _current_offboard_mission_index); + _mission_result.seq_current = _current_offboard_mission_index; publish_mission_result(); + + save_offboard_mission_state(); } void Mission::report_mission_finished() { - _mission_result.mission_finished = true; + _mission_result.finished = true; publish_mission_result(); } @@ -456,6 +623,6 @@ Mission::publish_mission_result() _mission_result_pub = orb_advertise(ORB_ID(mission_result), &_mission_result); } /* reset reached bool */ - _mission_result.mission_reached = false; - _mission_result.mission_finished = false; + _mission_result.reached = false; + _mission_result.finished = false; } diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h index 6e4761946..4da6a1155 100644 --- a/src/modules/navigator/mission.h +++ b/src/modules/navigator/mission.h @@ -65,25 +65,15 @@ class Navigator; class Mission : public MissionBlock { public: - /** - * Constructor - */ Mission(Navigator *navigator, const char *name); - /** - * Destructor - */ virtual ~Mission(); - /** - * This function is called while the mode is inactive - */ virtual void on_inactive(); - /** - * This function is called while the mode is active - */ - virtual bool on_active(struct position_setpoint_triplet_s *pos_sp_triplet); + virtual void on_activation(); + + virtual void on_active(); private: /** @@ -102,38 +92,26 @@ private: void advance_mission(); /** - * Set new mission items - */ - void set_mission_items(struct position_setpoint_triplet_s *pos_sp_triplet); - - /** - * Try to set the current position setpoint from an onboard mission item - * @return true if mission item successfully set - */ - bool is_current_onboard_mission_item_set(struct position_setpoint_s *current_pos_sp); - - /** - * Try to set the current position setpoint from an offboard mission item - * @return true if mission item successfully set + * Check distance to first waypoint (with lat/lon) + * @return true only if it's not too far from home (< MIS_DIST_1WP) */ - bool is_current_offboard_mission_item_set(struct position_setpoint_s *current_pos_sp); + bool check_dist_1wp(); /** - * Try to set the next position setpoint from an onboard mission item + * Set new mission items */ - void get_next_onboard_mission_item(struct position_setpoint_s *next_pos_sp); + void set_mission_items(); /** - * Try to set the next position setpoint from an offboard mission item + * Read current or next mission item from the dataman and watch out for DO_JUMPS + * @return true if successful */ - void get_next_offboard_mission_item(struct position_setpoint_s *next_pos_sp); + bool read_mission_item(bool onboard, bool is_current, struct mission_item_s *mission_item); /** - * Read a mission item from the dataman and watch out for DO_JUMPS - * @return true if successful + * Save current offboard mission state to dataman */ - bool read_mission_item(const dm_item_t dm_item, bool is_current, int *mission_index, - struct mission_item_s *new_mission_item); + void save_offboard_mission_state(); /** * Report that a mission item has been reached @@ -155,13 +133,17 @@ private: */ void publish_mission_result(); - control::BlockParamFloat _param_onboard_enabled; + control::BlockParamInt _param_onboard_enabled; + control::BlockParamFloat _param_takeoff_alt; + control::BlockParamFloat _param_dist_1wp; struct mission_s _onboard_mission; struct mission_s _offboard_mission; int _current_onboard_mission_index; int _current_offboard_mission_index; + bool _need_takeoff; + bool _takeoff; orb_advert_t _mission_result_pub; struct mission_result_s _mission_result; @@ -172,6 +154,9 @@ private: MISSION_TYPE_OFFBOARD } _mission_type; + bool _inited; + bool _dist_1wp_ok; + MissionFeasibilityChecker missionFeasiblityChecker; /**< class that checks if a mission is feasible */ }; diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index 26a573544..4adf77dce 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -47,6 +47,7 @@ #include <systemlib/err.h> #include <geo/geo.h> +#include <mavlink/mavlink_log.h> #include <uORB/uORB.h> @@ -56,11 +57,10 @@ MissionBlock::MissionBlock(Navigator *navigator, const char *name) : NavigatorMode(navigator, name), + _mission_item({0}), _waypoint_position_reached(false), _waypoint_yaw_reached(false), - _time_first_inside_orbit(0), - _mission_item({0}), - _mission_item_valid(false) + _time_first_inside_orbit(0) { } @@ -71,6 +71,10 @@ MissionBlock::~MissionBlock() bool MissionBlock::is_mission_item_reached() { + if (_mission_item.nav_cmd == NAV_CMD_IDLE) { + return false; + } + if (_mission_item.nav_cmd == NAV_CMD_LAND) { return _navigator->get_vstatus()->condition_landed; } @@ -84,7 +88,6 @@ MissionBlock::is_mission_item_reached() hrt_abstime now = hrt_absolute_time(); if (!_waypoint_position_reached) { - float dist = -1.0f; float dist_xy = -1.0f; float dist_z = -1.0f; @@ -201,44 +204,48 @@ MissionBlock::mission_item_to_position_setpoint(const struct mission_item_s *ite } void -MissionBlock::set_previous_pos_setpoint(struct position_setpoint_triplet_s *pos_sp_triplet) +MissionBlock::set_previous_pos_setpoint() { - /* reuse current setpoint as previous setpoint */ + struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); + if (pos_sp_triplet->current.valid) { memcpy(&pos_sp_triplet->previous, &pos_sp_triplet->current, sizeof(struct position_setpoint_s)); } } -bool -MissionBlock::set_loiter_item(struct position_setpoint_triplet_s *pos_sp_triplet) +void +MissionBlock::set_loiter_item(struct mission_item_s *item) { - /* don't change setpoint if 'can_loiter_at_sp' flag set */ - if (!(_navigator->get_can_loiter_at_sp() && pos_sp_triplet->current.valid)) { - /* use current position */ - pos_sp_triplet->current.lat = _navigator->get_global_position()->lat; - pos_sp_triplet->current.lon = _navigator->get_global_position()->lon; - pos_sp_triplet->current.alt = _navigator->get_global_position()->alt; - pos_sp_triplet->current.yaw = NAN; /* NAN means to use current yaw */ - - _navigator->set_can_loiter_at_sp(true); - } + if (_navigator->get_vstatus()->condition_landed) { + /* landed, don't takeoff, but switch to IDLE mode */ + item->nav_cmd = NAV_CMD_IDLE; - if (pos_sp_triplet->current.type != SETPOINT_TYPE_LOITER - || (fabsf(pos_sp_triplet->current.loiter_radius - _navigator->get_loiter_radius()) > FLT_EPSILON) - || pos_sp_triplet->current.loiter_direction != 1 - || pos_sp_triplet->previous.valid - || !pos_sp_triplet->current.valid - || pos_sp_triplet->next.valid) { - /* position setpoint triplet should be updated */ - pos_sp_triplet->current.type = SETPOINT_TYPE_LOITER; - pos_sp_triplet->current.loiter_radius = _navigator->get_loiter_radius(); - pos_sp_triplet->current.loiter_direction = 1; - - pos_sp_triplet->previous.valid = false; - pos_sp_triplet->current.valid = true; - pos_sp_triplet->next.valid = false; - return true; - } + } else { + item->nav_cmd = NAV_CMD_LOITER_UNLIMITED; + + struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); + + if (_navigator->get_can_loiter_at_sp() && pos_sp_triplet->current.valid) { + /* use current position setpoint */ + item->lat = pos_sp_triplet->current.lat; + item->lon = pos_sp_triplet->current.lon; + item->altitude = pos_sp_triplet->current.alt; - return false; + } else { + /* use current position */ + item->lat = _navigator->get_global_position()->lat; + item->lon = _navigator->get_global_position()->lon; + item->altitude = _navigator->get_global_position()->alt; + } + + item->altitude_is_relative = false; + item->yaw = NAN; + item->loiter_radius = _navigator->get_loiter_radius(); + item->loiter_direction = 1; + item->acceptance_radius = _navigator->get_acceptance_radius(); + item->time_inside = 0.0f; + item->pitch_min = 0.0f; + item->autocontinue = false; + item->origin = ORIGIN_ONBOARD; + } } diff --git a/src/modules/navigator/mission_block.h b/src/modules/navigator/mission_block.h index f99002752..adf50bc39 100644 --- a/src/modules/navigator/mission_block.h +++ b/src/modules/navigator/mission_block.h @@ -64,6 +64,7 @@ public: */ virtual ~MissionBlock(); +protected: /** * Check if mission item has been reached * @return true if successfully reached @@ -85,22 +86,17 @@ public: /** * Set previous position setpoint to current setpoint */ - void set_previous_pos_setpoint(struct position_setpoint_triplet_s *pos_sp_triplet); + void set_previous_pos_setpoint(); /** - * Set a loiter item, if possible reuse the position setpoint, otherwise take the current position - * - * @param the position setpoint triplet to set - * @return true if setpoint has changed + * Set a loiter mission item, if possible reuse the position setpoint, otherwise take the current position */ - bool set_loiter_item(position_setpoint_triplet_s *pos_sp_triplet); + void set_loiter_item(struct mission_item_s *item); + mission_item_s _mission_item; bool _waypoint_position_reached; bool _waypoint_yaw_reached; hrt_abstime _time_first_inside_orbit; - - mission_item_s _mission_item; - bool _mission_item_valid; }; #endif diff --git a/src/modules/navigator/mission_feasibility_checker.cpp b/src/modules/navigator/mission_feasibility_checker.cpp index dd7f4c801..606521f20 100644 --- a/src/modules/navigator/mission_feasibility_checker.cpp +++ b/src/modules/navigator/mission_feasibility_checker.cpp @@ -135,12 +135,15 @@ bool MissionFeasibilityChecker::checkHomePositionAltitude(dm_item_t dm_current, } } - if (home_alt > missionitem.altitude) { + /* calculate the global waypoint altitude */ + float wp_alt = (missionitem.altitude_is_relative) ? missionitem.altitude + home_alt : missionitem.altitude; + + if (home_alt > wp_alt) { if (throw_error) { - mavlink_log_info(_mavlink_fd, "Waypoint %d below home", i); + mavlink_log_critical(_mavlink_fd, "Rejecting Mission: Waypoint %d below home", i); return false; } else { - mavlink_log_info(_mavlink_fd, "#audio: warning waypoint %d below home", i); + mavlink_log_critical(_mavlink_fd, "Warning: Waypoint %d below home", i); return true; } } diff --git a/src/modules/navigator/mission_params.c b/src/modules/navigator/mission_params.c index 8692328db..d15e1e771 100644 --- a/src/modules/navigator/mission_params.c +++ b/src/modules/navigator/mission_params.c @@ -58,12 +58,27 @@ */ PARAM_DEFINE_FLOAT(MIS_TAKEOFF_ALT, 10.0f); - /** - * Enable onboard mission + * Enable persistent onboard mission storage + * + * When enabled, missions that have been uploaded by the GCS are stored + * and reloaded after reboot persistently. * * @min 0 * @max 1 * @group Mission */ -PARAM_DEFINE_INT32(MIS_ONBOARD_EN, 0); +PARAM_DEFINE_INT32(MIS_ONBOARD_EN, 1); + +/** + * Maximal horizontal distance from home to first waypoint + * + * Failsafe check to prevent running mission stored from previous flight at a new takeoff location. + * Set a value of zero or less to disable. The mission will not be started if the current + * waypoint is more distant than MIS_DIS_1WP from the current position. + * + * @min 0 + * @max 1000 + * @group Mission + */ +PARAM_DEFINE_FLOAT(MIS_DIST_1WP, 500); diff --git a/src/modules/navigator/module.mk b/src/modules/navigator/module.mk index a1e109030..b50198996 100644 --- a/src/modules/navigator/module.mk +++ b/src/modules/navigator/module.mk @@ -46,6 +46,7 @@ SRCS = navigator_main.cpp \ loiter.cpp \ rtl.cpp \ rtl_params.c \ + offboard.cpp \ mission_feasibility_checker.cpp \ geofence.cpp \ geofence_params.c @@ -53,3 +54,5 @@ SRCS = navigator_main.cpp \ INCLUDE_DIRS += $(MAVLINK_SRC)/include/mavlink MODULE_STACKSIZE = 1200 + +EXTRACXXFLAGS = -Weffc++ diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index 184ecd365..8edbb63b3 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -56,13 +56,14 @@ #include "mission.h" #include "loiter.h" #include "rtl.h" +#include "offboard.h" #include "geofence.h" /** * Number of navigation modes that need on_active/on_inactive calls * Currently: mission, loiter, and rtl */ -#define NAVIGATOR_MODE_ARRAY_SIZE 3 +#define NAVIGATOR_MODE_ARRAY_SIZE 4 class Navigator : public control::SuperBlock { @@ -103,15 +104,19 @@ public: * Setters */ void set_can_loiter_at_sp(bool can_loiter) { _can_loiter_at_sp = can_loiter; } + void set_position_setpoint_triplet_updated() { _pos_sp_triplet_updated = true; } /** * Getters */ struct vehicle_status_s* get_vstatus() { return &_vstatus; } + struct vehicle_control_mode_s* get_control_mode() { return &_control_mode; } struct vehicle_global_position_s* get_global_position() { return &_global_pos; } struct home_position_s* get_home_position() { return &_home_pos; } + struct position_setpoint_triplet_s* get_position_setpoint_triplet() { return &_pos_sp_triplet; } int get_onboard_mission_sub() { return _onboard_mission_sub; } int get_offboard_mission_sub() { return _offboard_mission_sub; } + int get_offboard_control_sp_sub() { return _offboard_control_sp_sub; } Geofence& get_geofence() { return _geofence; } bool get_can_loiter_at_sp() { return _can_loiter_at_sp; } float get_loiter_radius() { return _param_loiter_radius.get(); } @@ -129,6 +134,7 @@ private: int _home_pos_sub; /**< home position subscription */ int _vstatus_sub; /**< vehicle status subscription */ int _capabilities_sub; /**< notification of vehicle capabilities updates */ + int _offboard_control_sp_sub; /*** offboard control subscription */ int _control_mode_sub; /**< vehicle control mode subscription */ int _onboard_mission_sub; /**< onboard mission subscription */ int _offboard_mission_sub; /**< offboard mission subscription */ @@ -158,11 +164,12 @@ private: Mission _mission; /**< class that handles the missions */ Loiter _loiter; /**< class that handles loiter */ RTL _rtl; /**< class that handles RTL */ + Offboard _offboard; /**< class that handles offboard */ NavigatorMode *_navigation_mode_array[NAVIGATOR_MODE_ARRAY_SIZE]; /**< array of navigation modes */ bool _can_loiter_at_sp; /**< flags if current position SP can be used to loiter */ - bool _update_triplet; /**< flags if position SP triplet needs to be published */ + bool _pos_sp_triplet_updated; /**< flags if position SP triplet needs to be published */ control::BlockParamFloat _param_loiter_radius; /**< loiter radius for fixedwing */ control::BlockParamFloat _param_acceptance_radius; /**< acceptance for takeoff */ @@ -215,5 +222,11 @@ private: * Publish a new position setpoint triplet for position controllers */ void publish_position_setpoint_triplet(); + + /* this class has ptr data members, so it should not be copied, + * consequently the copy constructors are private. + */ + Navigator(const Navigator&); + Navigator operator=(const Navigator&); }; #endif diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 546602741..331a9a728 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -67,6 +67,7 @@ #include <uORB/topics/mission.h> #include <uORB/topics/fence.h> #include <uORB/topics/navigation_capabilities.h> +#include <uORB/topics/offboard_control_setpoint.h> #include <systemlib/err.h> #include <systemlib/systemlib.h> @@ -100,20 +101,22 @@ Navigator::Navigator() : _home_pos_sub(-1), _vstatus_sub(-1), _capabilities_sub(-1), + _offboard_control_sp_sub(-1), _control_mode_sub(-1), _onboard_mission_sub(-1), _offboard_mission_sub(-1), + _param_update_sub(-1), _pos_sp_triplet_pub(-1), - _vstatus({}), - _control_mode({}), - _global_pos({}), - _home_pos({}), - _mission_item({}), - _nav_caps({}), - _pos_sp_triplet({}), + _vstatus{}, + _control_mode{}, + _global_pos{}, + _home_pos{}, + _mission_item{}, + _nav_caps{}, + _pos_sp_triplet{}, _mission_item_valid(false), _loop_perf(perf_alloc(PC_ELAPSED, "navigator")), - _geofence({}), + _geofence{}, _geofence_violation_warning_sent(false), _fence_valid(false), _inside_fence(true), @@ -121,7 +124,9 @@ Navigator::Navigator() : _mission(this, "MIS"), _loiter(this, "LOI"), _rtl(this, "RTL"), - _update_triplet(false), + _offboard(this, "OFF"), + _can_loiter_at_sp(false), + _pos_sp_triplet_updated(false), _param_loiter_radius(this, "LOITER_RAD"), _param_acceptance_radius(this, "ACC_RAD") { @@ -129,6 +134,7 @@ Navigator::Navigator() : _navigation_mode_array[0] = &_mission; _navigation_mode_array[1] = &_loiter; _navigation_mode_array[2] = &_rtl; + _navigation_mode_array[3] = &_offboard; updateParams(); } @@ -241,6 +247,7 @@ Navigator::task_main() _onboard_mission_sub = orb_subscribe(ORB_ID(onboard_mission)); _offboard_mission_sub = orb_subscribe(ORB_ID(offboard_mission)); _param_update_sub = orb_subscribe(ORB_ID(parameter_update)); + _offboard_control_sp_sub = orb_subscribe(ORB_ID(offboard_control_setpoint)); /* copy all topics first time */ vehicle_status_update(); @@ -363,6 +370,9 @@ Navigator::task_main() break; case NAVIGATION_STATE_LAND: case NAVIGATION_STATE_TERMINATION: + case NAVIGATION_STATE_OFFBOARD: + _navigation_mode = &_offboard; + break; default: _navigation_mode = nullptr; _can_loiter_at_sp = false; @@ -371,24 +381,21 @@ Navigator::task_main() /* iterate through navigation modes and set active/inactive for each */ for(unsigned int i = 0; i < NAVIGATOR_MODE_ARRAY_SIZE; i++) { - if (_navigation_mode == _navigation_mode_array[i]) { - _update_triplet = _navigation_mode_array[i]->on_active(&_pos_sp_triplet); - } else { - _navigation_mode_array[i]->on_inactive(); - } + _navigation_mode_array[i]->run(_navigation_mode == _navigation_mode_array[i]); } /* if nothing is running, set position setpoint triplet invalid */ if (_navigation_mode == nullptr) { + // TODO publish empty sp only once _pos_sp_triplet.previous.valid = false; _pos_sp_triplet.current.valid = false; _pos_sp_triplet.next.valid = false; - _update_triplet = true; + _pos_sp_triplet_updated = true; } - if (_update_triplet) { + if (_pos_sp_triplet_updated) { publish_position_setpoint_triplet(); - _update_triplet = false; + _pos_sp_triplet_updated = false; } perf_end(_loop_perf); @@ -451,7 +458,7 @@ void Navigator::publish_position_setpoint_triplet() { /* update navigation state */ - /* TODO: set nav_state */ + _pos_sp_triplet.nav_state = _vstatus.nav_state; /* lazily publish the position setpoint triplet only once available */ if (_pos_sp_triplet_pub > 0) { diff --git a/src/modules/navigator/navigator_mode.cpp b/src/modules/navigator/navigator_mode.cpp index 25e767c2b..f43215665 100644 --- a/src/modules/navigator/navigator_mode.cpp +++ b/src/modules/navigator/navigator_mode.cpp @@ -33,12 +33,14 @@ /** * @file navigator_mode.cpp * - * Helper class for different modes in navigator + * Base class for different modes in navigator * * @author Julian Oes <julian@oes.ch> + * @author Anton Babushkin <anton.babushkin@me.com> */ #include "navigator_mode.h" +#include "navigator.h" NavigatorMode::NavigatorMode(Navigator *navigator, const char *name) : SuperBlock(NULL, name), @@ -56,15 +58,38 @@ NavigatorMode::~NavigatorMode() } void +NavigatorMode::run(bool active) { + if (active) { + if (_first_run) { + /* first run */ + _first_run = false; + on_activation(); + + } else { + /* periodic updates when active */ + on_active(); + } + + } else { + /* periodic updates when inactive */ + _first_run = true; + on_inactive(); + } +} + +void NavigatorMode::on_inactive() { - _first_run = true; } -bool -NavigatorMode::on_active(struct position_setpoint_triplet_s *pos_sp_triplet) +void +NavigatorMode::on_activation() +{ + /* invalidate position setpoint by default */ + _navigator->get_position_setpoint_triplet()->current.valid = false; +} + +void +NavigatorMode::on_active() { - pos_sp_triplet->current.valid = false; - _first_run = false; - return false; } diff --git a/src/modules/navigator/navigator_mode.h b/src/modules/navigator/navigator_mode.h index cbb53d91b..de5545dcb 100644 --- a/src/modules/navigator/navigator_mode.h +++ b/src/modules/navigator/navigator_mode.h @@ -33,9 +33,10 @@ /** * @file navigator_mode.h * - * Helper class for different modes in navigator + * Base class for different modes in navigator * * @author Julian Oes <julian@oes.ch> + * @author Anton Babushkin <anton.babushkin@me.com> */ #ifndef NAVIGATOR_MODE_H @@ -65,22 +66,34 @@ public: */ virtual ~NavigatorMode(); + void run(bool active); + /** * This function is called while the mode is inactive */ virtual void on_inactive(); /** + * This function is called one time when mode become active, poos_sp_triplet must be initialized here + */ + virtual void on_activation(); + + /** * This function is called while the mode is active - * - * @param position setpoint triplet to set - * @return true if position setpoint triplet has been changed */ - virtual bool on_active(struct position_setpoint_triplet_s *pos_sp_triplet); + virtual void on_active(); protected: Navigator *_navigator; + +private: bool _first_run; + + /* this class has ptr data members, so it should not be copied, + * consequently the copy constructors are private. + */ + NavigatorMode(const NavigatorMode&); + NavigatorMode operator=(const NavigatorMode&); }; #endif diff --git a/src/modules/navigator/offboard.cpp b/src/modules/navigator/offboard.cpp new file mode 100644 index 000000000..dcb5c6000 --- /dev/null +++ b/src/modules/navigator/offboard.cpp @@ -0,0 +1,129 @@ +/**************************************************************************** + * + * 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 offboard.cpp + * + * Helper class for offboard commands + * + * @author Julian Oes <julian@oes.ch> + */ + +#include <string.h> +#include <stdlib.h> +#include <stdbool.h> +#include <math.h> +#include <fcntl.h> + +#include <mavlink/mavlink_log.h> +#include <systemlib/err.h> + +#include <uORB/uORB.h> +#include <uORB/topics/position_setpoint_triplet.h> + +#include "navigator.h" +#include "offboard.h" + +Offboard::Offboard(Navigator *navigator, const char *name) : + NavigatorMode(navigator, name), + _offboard_control_sp({0}) +{ + /* load initial params */ + updateParams(); + /* initial reset */ + on_inactive(); +} + +Offboard::~Offboard() +{ +} + +void +Offboard::on_inactive() +{ +} + +void +Offboard::on_activation() +{ +} + +void +Offboard::on_active() +{ + struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); + + bool updated; + orb_check(_navigator->get_offboard_control_sp_sub(), &updated); + if (updated) { + update_offboard_control_setpoint(); + } + + /* copy offboard setpoints to the corresponding topics */ + if (_navigator->get_control_mode()->flag_control_position_enabled + && _offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_POSITION) { + /* position control */ + pos_sp_triplet->current.x = _offboard_control_sp.p1; + pos_sp_triplet->current.y = _offboard_control_sp.p2; + pos_sp_triplet->current.yaw = _offboard_control_sp.p3; + pos_sp_triplet->current.z = -_offboard_control_sp.p4; + + pos_sp_triplet->current.type = SETPOINT_TYPE_OFFBOARD; + pos_sp_triplet->current.valid = true; + pos_sp_triplet->current.position_valid = true; + + _navigator->set_position_setpoint_triplet_updated(); + + } else if (_navigator->get_control_mode()->flag_control_velocity_enabled + && _offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY) { + /* velocity control */ + pos_sp_triplet->current.vx = _offboard_control_sp.p2; + pos_sp_triplet->current.vy = _offboard_control_sp.p1; + pos_sp_triplet->current.yawspeed = _offboard_control_sp.p3; + pos_sp_triplet->current.vz = _offboard_control_sp.p4; + + pos_sp_triplet->current.type = SETPOINT_TYPE_OFFBOARD; + pos_sp_triplet->current.valid = true; + pos_sp_triplet->current.velocity_valid = true; + + _navigator->set_position_setpoint_triplet_updated(); + } + +} + + +void +Offboard::update_offboard_control_setpoint() +{ + orb_copy(ORB_ID(offboard_control_setpoint), _navigator->get_offboard_control_sp_sub(), &_offboard_control_sp); + +} diff --git a/src/modules/mavlink/util.h b/src/modules/navigator/offboard.h index 5ca9a085d..66b923bdb 100644 --- a/src/modules/mavlink/util.h +++ b/src/modules/navigator/offboard.h @@ -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 @@ -30,24 +30,43 @@ * POSSIBILITY OF SUCH DAMAGE. * ****************************************************************************/ - /** - * @file util.h - * Utility and helper functions and data. + * @file offboard.h + * + * Helper class for offboard commands + * + * @author Julian Oes <julian@oes.ch> */ -#pragma once +#ifndef NAVIGATOR_OFFBOARD_H +#define NAVIGATOR_OFFBOARD_H -/** MAVLink communications channel */ -extern uint8_t chan; +#include <controllib/blocks.hpp> +#include <controllib/block/BlockParam.hpp> -/** Shutdown marker */ -extern volatile bool thread_should_exit; +#include <uORB/uORB.h> +#include <uORB/topics/offboard_control_setpoint.h> -/** Waypoint storage */ -extern mavlink_wpm_storage *wpm; +#include "navigator_mode.h" -/** - * Translate the custom state into standard mavlink modes and state. - */ -extern void get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_base_mode, uint32_t *mavlink_custom_mode); +class Navigator; + +class Offboard : public NavigatorMode +{ +public: + Offboard(Navigator *navigator, const char *name); + + ~Offboard(); + + virtual void on_inactive(); + + virtual void on_activation(); + + virtual void on_active(); +private: + void update_offboard_control_setpoint(); + + struct offboard_control_setpoint_s _offboard_control_sp; +}; + +#endif diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp index 597a2c0ec..142a73409 100644 --- a/src/modules/navigator/rtl.cpp +++ b/src/modules/navigator/rtl.cpp @@ -75,62 +75,57 @@ RTL::~RTL() void RTL::on_inactive() { - _first_run = true; - /* reset RTL state only if setpoint moved */ if (!_navigator->get_can_loiter_at_sp()) { _rtl_state = RTL_STATE_NONE; } } -bool -RTL::on_active(struct position_setpoint_triplet_s *pos_sp_triplet) +void +RTL::on_activation() { - bool updated = false; - - if (_first_run) { - _first_run = false; - - /* decide where to enter the RTL procedure when we switch into it */ - if (_rtl_state == RTL_STATE_NONE) { - /* for safety reasons don't go into RTL if landed */ - if (_navigator->get_vstatus()->condition_landed) { - _rtl_state = RTL_STATE_LANDED; - mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: no RTL when landed"); - - /* if lower than return altitude, climb up first */ - } else if (_navigator->get_global_position()->alt < _navigator->get_home_position()->alt - + _param_return_alt.get()) { - _rtl_state = RTL_STATE_CLIMB; - - /* otherwise go straight to return */ - } else { - /* set altitude setpoint to current altitude */ - _rtl_state = RTL_STATE_RETURN; - _mission_item.altitude_is_relative = false; - _mission_item.altitude = _navigator->get_global_position()->alt; - } + /* decide where to enter the RTL procedure when we switch into it */ + if (_rtl_state == RTL_STATE_NONE) { + /* for safety reasons don't go into RTL if landed */ + if (_navigator->get_vstatus()->condition_landed) { + _rtl_state = RTL_STATE_LANDED; + mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: no RTL when landed"); + + /* if lower than return altitude, climb up first */ + } else if (_navigator->get_global_position()->alt < _navigator->get_home_position()->alt + + _param_return_alt.get()) { + _rtl_state = RTL_STATE_CLIMB; + + /* otherwise go straight to return */ + } else { + /* set altitude setpoint to current altitude */ + _rtl_state = RTL_STATE_RETURN; + _mission_item.altitude_is_relative = false; + _mission_item.altitude = _navigator->get_global_position()->alt; } + } - set_rtl_item(pos_sp_triplet); - updated = true; + set_rtl_item(); +} - } else if (_rtl_state != RTL_STATE_LANDED && is_mission_item_reached()) { +void +RTL::on_active() +{ + if (_rtl_state != RTL_STATE_LANDED && is_mission_item_reached()) { advance_rtl(); - set_rtl_item(pos_sp_triplet); - updated = true; + set_rtl_item(); } - - return updated; } void -RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet) +RTL::set_rtl_item() { + struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); + /* make sure we have the latest params */ updateParams(); - set_previous_pos_setpoint(pos_sp_triplet); + set_previous_pos_setpoint(); _navigator->set_can_loiter_at_sp(false); switch (_rtl_state) { @@ -277,11 +272,13 @@ RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet) break; } + reset_mission_item_reached(); + /* convert mission item to current position setpoint and make it valid */ mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current); - reset_mission_item_reached(); - pos_sp_triplet->current.valid = true; pos_sp_triplet->next.valid = false; + + _navigator->set_position_setpoint_triplet_updated(); } void diff --git a/src/modules/navigator/rtl.h b/src/modules/navigator/rtl.h index b4b729e89..5928f1f07 100644 --- a/src/modules/navigator/rtl.h +++ b/src/modules/navigator/rtl.h @@ -57,35 +57,21 @@ class Navigator; class RTL : public MissionBlock { public: - /** - * Constructor - */ RTL(Navigator *navigator, const char *name); - /** - * Destructor - */ ~RTL(); - /** - * This function is called while the mode is inactive - */ - void on_inactive(); + virtual void on_inactive(); - /** - * This function is called while the mode is active - * - * @param position setpoint triplet that needs to be set - * @return true if updated - */ - bool on_active(position_setpoint_triplet_s *pos_sp_triplet); + virtual void on_activation(); + virtual void on_active(); private: /** * Set the RTL item */ - void set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet); + void set_rtl_item(); /** * Move to next RTL item 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 db1ce79ca..05eae047c 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -915,6 +915,10 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) memcpy(x_est_prev, x_est, sizeof(x_est)); memcpy(y_est_prev, y_est, sizeof(y_est)); } + } else { + /* gradually reset xy velocity estimates */ + inertial_filter_correct(-x_est[1], dt, x_est, 1, params.w_xy_res_v); + inertial_filter_correct(-y_est[1], dt, y_est, 1, params.w_xy_res_v); } /* detect land */ @@ -930,6 +934,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) landed = false; landed_time = 0; } + /* reset xy velocity estimates when landed */ + x_est[1] = 0.0f; + y_est[1] = 0.0f; } else { if (alt_disp2 < land_disp2 && thrust < params.land_thr) { diff --git a/src/modules/position_estimator_inav/position_estimator_inav_params.c b/src/modules/position_estimator_inav/position_estimator_inav_params.c index d88419c25..98b31d17b 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_params.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_params.c @@ -40,21 +40,196 @@ #include "position_estimator_inav_params.h" +/** + * Z axis weight for barometer + * + * Weight (cutoff frequency) for barometer altitude measurements. + * + * @min 0.0 + * @max 10.0 + * @group Position Estimator INAV + */ PARAM_DEFINE_FLOAT(INAV_W_Z_BARO, 0.5f); + +/** + * Z axis weight for GPS + * + * Weight (cutoff frequency) for GPS altitude measurements. GPS altitude data is very noisy and should be used only as slow correction for baro offset. + * + * @min 0.0 + * @max 10.0 + * @group Position Estimator INAV + */ PARAM_DEFINE_FLOAT(INAV_W_Z_GPS_P, 0.005f); + +/** + * Z axis weight for sonar + * + * Weight (cutoff frequency) for sonar measurements. + * + * @min 0.0 + * @max 10.0 + * @group Position Estimator INAV + */ PARAM_DEFINE_FLOAT(INAV_W_Z_SONAR, 3.0f); + +/** + * XY axis weight for GPS position + * + * Weight (cutoff frequency) for GPS position measurements. + * + * @min 0.0 + * @max 10.0 + * @group Position Estimator INAV + */ PARAM_DEFINE_FLOAT(INAV_W_XY_GPS_P, 1.0f); + +/** + * XY axis weight for GPS velocity + * + * Weight (cutoff frequency) for GPS velocity measurements. + * + * @min 0.0 + * @max 10.0 + * @group Position Estimator INAV + */ PARAM_DEFINE_FLOAT(INAV_W_XY_GPS_V, 2.0f); + +/** + * XY axis weight for optical flow + * + * Weight (cutoff frequency) for optical flow (velocity) measurements. + * + * @min 0.0 + * @max 10.0 + * @group Position Estimator INAV + */ PARAM_DEFINE_FLOAT(INAV_W_XY_FLOW, 5.0f); + +/** + * XY axis weight for resetting velocity + * + * When velocity sources lost slowly decrease estimated horizontal velocity with this weight. + * + * @min 0.0 + * @max 10.0 + * @group Position Estimator INAV + */ +PARAM_DEFINE_FLOAT(INAV_W_XY_RES_V, 0.5f); + +/** + * XY axis weight factor for GPS when optical flow available + * + * When optical flow data available, multiply GPS weights (for position and velocity) by this factor. + * + * @min 0.0 + * @max 1.0 + * @group Position Estimator INAV + */ PARAM_DEFINE_FLOAT(INAV_W_GPS_FLOW, 0.1f); + +/** + * Accelerometer bias estimation weight + * + * Weight (cutoff frequency) for accelerometer bias estimation. 0 to disable. + * + * @min 0.0 + * @max 0.1 + * @group Position Estimator INAV + */ PARAM_DEFINE_FLOAT(INAV_W_ACC_BIAS, 0.05f); + +/** + * Optical flow scale factor + * + * Factor to convert raw optical flow (in pixels) to radians [rad/px]. + * + * @min 0.0 + * @max 1.0 + * @unit rad/px + * @group Position Estimator INAV + */ PARAM_DEFINE_FLOAT(INAV_FLOW_K, 0.15f); + +/** + * Minimal acceptable optical flow quality + * + * 0 - lowest quality, 1 - best quality. + * + * @min 0.0 + * @max 1.0 + * @group Position Estimator INAV + */ PARAM_DEFINE_FLOAT(INAV_FLOW_Q_MIN, 0.5f); + +/** + * Weight for sonar filter + * + * Sonar filter detects spikes on sonar measurements and used to detect new surface level. + * + * @min 0.0 + * @max 1.0 + * @group Position Estimator INAV + */ PARAM_DEFINE_FLOAT(INAV_SONAR_FILT, 0.05f); + +/** + * Sonar maximal error for new surface + * + * If sonar measurement error is larger than this value it skiped (spike) or accepted as new surface level (if offset is stable). + * + * @min 0.0 + * @max 1.0 + * @unit m + * @group Position Estimator INAV + */ PARAM_DEFINE_FLOAT(INAV_SONAR_ERR, 0.5f); + +/** + * Land detector time + * + * Vehicle assumed landed if no altitude changes happened during this time on low throttle. + * + * @min 0.0 + * @max 10.0 + * @unit s + * @group Position Estimator INAV + */ PARAM_DEFINE_FLOAT(INAV_LAND_T, 3.0f); + +/** + * Land detector altitude dispersion threshold + * + * Dispersion threshold for triggering land detector. + * + * @min 0.0 + * @max 10.0 + * @unit m + * @group Position Estimator INAV + */ PARAM_DEFINE_FLOAT(INAV_LAND_DISP, 0.7f); + +/** + * Land detector throttle threshold + * + * Value should be lower than minimal hovering thrust. Half of it is good choice. + * + * @min 0.0 + * @max 1.0 + * @group Position Estimator INAV + */ PARAM_DEFINE_FLOAT(INAV_LAND_THR, 0.2f); + +/** + * GPS delay + * + * GPS delay compensation + * + * @min 0.0 + * @max 1.0 + * @unit s + * @group Position Estimator INAV + */ PARAM_DEFINE_FLOAT(INAV_DELAY_GPS, 0.2f); int parameters_init(struct position_estimator_inav_param_handles *h) @@ -65,6 +240,7 @@ int parameters_init(struct position_estimator_inav_param_handles *h) h->w_xy_gps_p = param_find("INAV_W_XY_GPS_P"); h->w_xy_gps_v = param_find("INAV_W_XY_GPS_V"); h->w_xy_flow = param_find("INAV_W_XY_FLOW"); + h->w_xy_res_v = param_find("INAV_W_XY_RES_V"); h->w_gps_flow = param_find("INAV_W_GPS_FLOW"); h->w_acc_bias = param_find("INAV_W_ACC_BIAS"); h->flow_k = param_find("INAV_FLOW_K"); @@ -87,6 +263,7 @@ int parameters_update(const struct position_estimator_inav_param_handles *h, str param_get(h->w_xy_gps_p, &(p->w_xy_gps_p)); param_get(h->w_xy_gps_v, &(p->w_xy_gps_v)); param_get(h->w_xy_flow, &(p->w_xy_flow)); + param_get(h->w_xy_res_v, &(p->w_xy_res_v)); param_get(h->w_gps_flow, &(p->w_gps_flow)); param_get(h->w_acc_bias, &(p->w_acc_bias)); param_get(h->flow_k, &(p->flow_k)); diff --git a/src/modules/position_estimator_inav/position_estimator_inav_params.h b/src/modules/position_estimator_inav/position_estimator_inav_params.h index df1cfaa71..423f0d879 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_params.h +++ b/src/modules/position_estimator_inav/position_estimator_inav_params.h @@ -47,6 +47,7 @@ struct position_estimator_inav_params { float w_xy_gps_p; float w_xy_gps_v; float w_xy_flow; + float w_xy_res_v; float w_gps_flow; float w_acc_bias; float flow_k; @@ -66,6 +67,7 @@ struct position_estimator_inav_param_handles { param_t w_xy_gps_p; param_t w_xy_gps_v; param_t w_xy_flow; + param_t w_xy_res_v; param_t w_gps_flow; param_t w_acc_bias; param_t flow_k; diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c index 62e6b12cb..185cb20dd 100644 --- a/src/modules/px4iofirmware/controls.c +++ b/src/modules/px4iofirmware/controls.c @@ -47,8 +47,8 @@ #include "px4io.h" #define RC_FAILSAFE_TIMEOUT 2000000 /**< two seconds failsafe timeout */ -#define RC_CHANNEL_HIGH_THRESH 5000 -#define RC_CHANNEL_LOW_THRESH -5000 +#define RC_CHANNEL_HIGH_THRESH 5000 /* 75% threshold */ +#define RC_CHANNEL_LOW_THRESH -8000 /* 10% threshold */ static bool ppm_input(uint16_t *values, uint16_t *num_values, uint16_t *frame_len); diff --git a/src/modules/px4iofirmware/i2c.c b/src/modules/px4iofirmware/i2c.c index 76762c0fc..6d1d1fc2d 100644 --- a/src/modules/px4iofirmware/i2c.c +++ b/src/modules/px4iofirmware/i2c.c @@ -64,12 +64,15 @@ #define rCCR REG(STM32_I2C_CCR_OFFSET) #define rTRISE REG(STM32_I2C_TRISE_OFFSET) +void i2c_reset(void); static int i2c_interrupt(int irq, void *context); static void i2c_rx_setup(void); static void i2c_tx_setup(void); static void i2c_rx_complete(void); static void i2c_tx_complete(void); +#ifdef DEBUG static void i2c_dump(void); +#endif static DMA_HANDLE rx_dma; static DMA_HANDLE tx_dma; diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index 2f721bf1e..606c639f9 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -111,7 +111,7 @@ mixer_tick(void) r_status_flags |= PX4IO_P_STATUS_FLAGS_FMU_OK; } - /* default to failsafe mixing */ + /* default to failsafe mixing - it will be forced below if flag is set */ source = MIX_FAILSAFE; /* @@ -155,6 +155,13 @@ mixer_tick(void) } /* + * Check if we should force failsafe - and do it if we have to + */ + if (r_setup_arming & PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE) { + source = MIX_FAILSAFE; + } + + /* * Set failsafe status flag depending on mixing source */ if (source == MIX_FAILSAFE) { diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h index d5a6b1ec4..050783687 100644 --- a/src/modules/px4iofirmware/protocol.h +++ b/src/modules/px4iofirmware/protocol.h @@ -179,6 +179,7 @@ #define PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE (1 << 5) /* Output of PWM right after startup enabled to help ESCs initialize and prevent them from beeping */ #define PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED (1 << 6) /* Disable the IO-internal evaluation of the RC */ #define PX4IO_P_SETUP_ARMING_LOCKDOWN (1 << 7) /* If set, the system operates normally, but won't actuate any servos */ +#define PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE (1 << 8) /* If set, the system will always output the failsafe values */ #define PX4IO_P_SETUP_PWM_RATES 2 /* bitmask, 0 = low rate, 1 = high rate */ #define PX4IO_P_SETUP_PWM_DEFAULTRATE 3 /* 'low' PWM frame output rate in Hz */ @@ -253,6 +254,10 @@ enum { /* DSM bind states */ /* PWM failsafe values - zero disables the output */ #define PX4IO_PAGE_FAILSAFE_PWM 55 /**< 0..CONFIG_ACTUATOR_COUNT-1 */ +/* PWM failsafe values - zero disables the output */ +#define PX4IO_PAGE_SENSORS 56 /**< Sensors connected to PX4IO */ +#define PX4IO_P_SENSORS_ALTITUDE 0 /**< Altitude of an external sensor (HoTT or S.BUS2) */ + /* Debug and test page - not used in normal operation */ #define PX4IO_PAGE_TEST 127 #define PX4IO_P_TEST_LED 0 /**< set the amber LED on/off */ diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h index ca175bfbc..b00a96717 100644 --- a/src/modules/px4iofirmware/px4io.h +++ b/src/modules/px4iofirmware/px4io.h @@ -219,8 +219,8 @@ extern bool dsm_input(uint16_t *values, uint16_t *num_values); extern void dsm_bind(uint16_t cmd, int pulses); extern int sbus_init(const char *device); extern bool sbus_input(uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, uint16_t max_channels); -extern bool sbus1_output(uint16_t *values, uint16_t num_values); -extern bool sbus2_output(uint16_t *values, uint16_t num_values); +extern void sbus1_output(uint16_t *values, uint16_t num_values); +extern void sbus2_output(uint16_t *values, uint16_t num_values); /** global debug level for isr_debug() */ extern volatile uint8_t debug_level; diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index 50108ea1b..43161aa70 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -189,7 +189,8 @@ volatile uint16_t r_page_setup[] = PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM | \ PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE | \ PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED | \ - PX4IO_P_SETUP_ARMING_LOCKDOWN) + PX4IO_P_SETUP_ARMING_LOCKDOWN | \ + PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE) #define PX4IO_P_SETUP_RATES_VALID ((1 << PX4IO_SERVO_COUNT) - 1) #define PX4IO_P_SETUP_RELAYS_VALID ((1 << PX4IO_RELAY_CHANNELS) - 1) @@ -711,7 +712,7 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val { #define SELECT_PAGE(_page_name) \ do { \ - *values = &_page_name[0]; \ + *values = (uint16_t *)&_page_name[0]; \ *num_values = sizeof(_page_name) / sizeof(_page_name[0]); \ } while(0) @@ -738,30 +739,19 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val { /* * Coefficients here derived by measurement of the 5-16V - * range on one unit: + * range on one unit, validated on sample points of another unit * - * V counts - * 5 1001 - * 6 1219 - * 7 1436 - * 8 1653 - * 9 1870 - * 10 2086 - * 11 2303 - * 12 2522 - * 13 2738 - * 14 2956 - * 15 3172 - * 16 3389 + * Data in Tools/tests-host/data folder. * - * slope = 0.0046067 - * intercept = 0.3863 + * measured slope = 0.004585267878277 (int: 4585) + * nominal theoretic slope: 0.00459340659 (int: 4593) + * intercept = 0.016646394188076 (int: 16646) + * nominal theoretic intercept: 0.00 (int: 0) * - * Intercept corrected for best results @ 12V. */ unsigned counts = adc_measure(ADC_VBATT); if (counts != 0xffff) { - unsigned mV = (4150 + (counts * 46)) / 10 - 200; + unsigned mV = (166460 + (counts * 45934)) / 10000; unsigned corrected = (mV * r_page_setup[PX4IO_P_SETUP_VBATT_SCALE]) / 10000; r_page_status[PX4IO_P_STATUS_VBATT] = corrected; diff --git a/src/modules/px4iofirmware/sbus.c b/src/modules/px4iofirmware/sbus.c index 70ccab180..0f0414148 100644 --- a/src/modules/px4iofirmware/sbus.c +++ b/src/modules/px4iofirmware/sbus.c @@ -116,14 +116,14 @@ sbus_init(const char *device) return sbus_fd; } -bool +void sbus1_output(uint16_t *values, uint16_t num_values) { char a = 'A'; write(sbus_fd, &a, 1); } -bool +void sbus2_output(uint16_t *values, uint16_t num_values) { char b = 'B'; diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 9b071ff99..6c4b49452 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -74,6 +74,7 @@ #include <uORB/topics/vehicle_global_position.h> #include <uORB/topics/position_setpoint_triplet.h> #include <uORB/topics/vehicle_gps_position.h> +#include <uORB/topics/satellite_info.h> #include <uORB/topics/vehicle_vicon_position.h> #include <uORB/topics/vehicle_global_velocity_setpoint.h> #include <uORB/topics/optical_flow.h> @@ -141,6 +142,8 @@ PARAM_DEFINE_INT32(SDLOG_EXT, -1); fds[fdsc_count].events = POLLIN; \ fdsc_count++; +#define MIN(X,Y) ((X) < (Y) ? (X) : (Y)) + static bool main_thread_should_exit = false; /**< Deamon exit flag */ static bool thread_running = false; /**< Deamon status flag */ static int deamon_task; /**< Handle of deamon task / thread */ @@ -944,6 +947,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct tecs_status_s tecs_status; struct system_power_s system_power; struct servorail_status_s servorail_status; + struct satellite_info_s sat_info; struct wind_estimate_s wind_estimate; } buf; @@ -975,7 +979,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct log_GVSP_s log_GVSP; struct log_BATT_s log_BATT; struct log_DIST_s log_DIST; - struct log_TELE_s log_TELE; + struct log_TEL_s log_TEL; struct log_EST0_s log_EST0; struct log_EST1_s log_EST1; struct log_PWR_s log_PWR; @@ -1007,6 +1011,7 @@ int sdlog2_thread_main(int argc, char *argv[]) int global_pos_sub; int triplet_sub; int gps_pos_sub; + int sat_info_sub; int vicon_pos_sub; int flow_sub; int rc_sub; @@ -1014,7 +1019,7 @@ int sdlog2_thread_main(int argc, char *argv[]) int esc_sub; int global_vel_sp_sub; int battery_sub; - int telemetry_sub; + int telemetry_subs[TELEMETRY_STATUS_ORB_ID_NUM]; int range_finder_sub; int estimator_status_sub; int tecs_status_sub; @@ -1026,6 +1031,7 @@ int sdlog2_thread_main(int argc, char *argv[]) subs.cmd_sub = orb_subscribe(ORB_ID(vehicle_command)); subs.status_sub = orb_subscribe(ORB_ID(vehicle_status)); subs.gps_pos_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); + subs.sat_info_sub = orb_subscribe(ORB_ID(satellite_info)); subs.sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); subs.att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); subs.att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); @@ -1043,7 +1049,9 @@ int sdlog2_thread_main(int argc, char *argv[]) subs.esc_sub = orb_subscribe(ORB_ID(esc_status)); subs.global_vel_sp_sub = orb_subscribe(ORB_ID(vehicle_global_velocity_setpoint)); subs.battery_sub = orb_subscribe(ORB_ID(battery_status)); - subs.telemetry_sub = orb_subscribe(ORB_ID(telemetry_status)); + for (int i = 0; i < TELEMETRY_STATUS_ORB_ID_NUM; i++) { + subs.telemetry_subs[i] = orb_subscribe(telemetry_status_orb_id[i]); + } subs.range_finder_sub = orb_subscribe(ORB_ID(sensor_range_finder)); subs.estimator_status_sub = orb_subscribe(ORB_ID(estimator_status)); subs.tecs_status_sub = orb_subscribe(ORB_ID(tecs_status)); @@ -1065,12 +1073,21 @@ int sdlog2_thread_main(int argc, char *argv[]) hrt_abstime magnetometer_timestamp = 0; hrt_abstime barometer_timestamp = 0; hrt_abstime differential_pressure_timestamp = 0; + hrt_abstime gyro1_timestamp = 0; + hrt_abstime accelerometer1_timestamp = 0; + hrt_abstime magnetometer1_timestamp = 0; + hrt_abstime gyro2_timestamp = 0; + hrt_abstime accelerometer2_timestamp = 0; + hrt_abstime magnetometer2_timestamp = 0; + + /* initialize calculated mean SNR */ + float snr_mean = 0.0f; /* enable logging on start if needed */ if (log_on_start) { /* check GPS topic to get GPS time */ if (log_name_timestamp) { - if (copy_if_updated(ORB_ID(vehicle_gps_position), subs.gps_pos_sub, &buf_gps_pos)) { + if (!orb_copy(ORB_ID(vehicle_gps_position), subs.gps_pos_sub, &buf_gps_pos)) { gps_time = buf_gps_pos.time_gps_usec; } } @@ -1128,14 +1145,6 @@ int sdlog2_thread_main(int argc, char *argv[]) /* --- GPS POSITION - UNIT #1 --- */ if (gps_pos_updated) { - float snr_mean = 0.0f; - - for (unsigned i = 0; i < buf_gps_pos.satellites_visible; i++) { - snr_mean += buf_gps_pos.satellite_snr[i]; - } - - snr_mean /= buf_gps_pos.satellites_visible; - log_msg.msg_type = LOG_GPS_MSG; log_msg.body.log_GPS.gps_time = buf_gps_pos.time_gps_usec; log_msg.body.log_GPS.fix_type = buf_gps_pos.fix_type; @@ -1148,44 +1157,55 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.body.log_GPS.vel_e = buf_gps_pos.vel_e_m_s; log_msg.body.log_GPS.vel_d = buf_gps_pos.vel_d_m_s; log_msg.body.log_GPS.cog = buf_gps_pos.cog_rad; - log_msg.body.log_GPS.sats = buf_gps_pos.satellites_visible; + log_msg.body.log_GPS.sats = buf_gps_pos.satellites_used; log_msg.body.log_GPS.snr_mean = snr_mean; log_msg.body.log_GPS.noise_per_ms = buf_gps_pos.noise_per_ms; log_msg.body.log_GPS.jamming_indicator = buf_gps_pos.jamming_indicator; LOGBUFFER_WRITE_AND_COUNT(GPS); + } + + /* --- SATELLITE INFO - UNIT #1 --- */ + if (_extended_logging) { + + if (copy_if_updated(ORB_ID(satellite_info), subs.sat_info_sub, &buf.sat_info)) { - if (_extended_logging) { /* log the SNR of each satellite for a detailed view of signal quality */ - unsigned gps_msg_max_snr = sizeof(buf_gps_pos.satellite_snr) / sizeof(buf_gps_pos.satellite_snr[0]); + unsigned sat_info_count = MIN(buf.sat_info.count, sizeof(buf.sat_info.snr) / sizeof(buf.sat_info.snr[0])); unsigned log_max_snr = sizeof(log_msg.body.log_GS0A.satellite_snr) / sizeof(log_msg.body.log_GS0A.satellite_snr[0]); log_msg.msg_type = LOG_GS0A_MSG; memset(&log_msg.body.log_GS0A, 0, sizeof(log_msg.body.log_GS0A)); - /* fill set A */ - for (unsigned i = 0; i < gps_msg_max_snr; i++) { + snr_mean = 0.0f; + + /* fill set A and calculate mean SNR */ + for (unsigned i = 0; i < sat_info_count; i++) { - int satindex = buf_gps_pos.satellite_prn[i] - 1; + snr_mean += buf.sat_info.snr[i]; + + int satindex = buf.sat_info.svid[i] - 1; /* handles index exceeding and wraps to to arithmetic errors */ if ((satindex >= 0) && (satindex < (int)log_max_snr)) { /* map satellites by their ID so that logs from two receivers can be compared */ - log_msg.body.log_GS0A.satellite_snr[satindex] = buf_gps_pos.satellite_snr[i]; + log_msg.body.log_GS0A.satellite_snr[satindex] = buf.sat_info.snr[i]; } } LOGBUFFER_WRITE_AND_COUNT(GS0A); + snr_mean /= sat_info_count; log_msg.msg_type = LOG_GS0B_MSG; memset(&log_msg.body.log_GS0B, 0, sizeof(log_msg.body.log_GS0B)); + /* fill set B */ - for (unsigned i = 0; i < gps_msg_max_snr; i++) { + for (unsigned i = 0; i < sat_info_count; i++) { /* get second bank of satellites, thus deduct bank size from index */ - int satindex = buf_gps_pos.satellite_prn[i] - 1 - log_max_snr; + int satindex = buf.sat_info.svid[i] - 1 - log_max_snr; /* handles index exceeding and wraps to to arithmetic errors */ if ((satindex >= 0) && (satindex < (int)log_max_snr)) { /* map satellites by their ID so that logs from two receivers can be compared */ - log_msg.body.log_GS0B.satellite_snr[satindex] = buf_gps_pos.satellite_snr[i]; + log_msg.body.log_GS0B.satellite_snr[satindex] = buf.sat_info.snr[i]; } } LOGBUFFER_WRITE_AND_COUNT(GS0B); @@ -1195,6 +1215,8 @@ int sdlog2_thread_main(int argc, char *argv[]) /* --- SENSOR COMBINED --- */ if (copy_if_updated(ORB_ID(sensor_combined), subs.sensor_sub, &buf.sensor)) { bool write_IMU = false; + bool write_IMU1 = false; + bool write_IMU2 = false; bool write_SENS = false; if (buf.sensor.timestamp != gyro_timestamp) { @@ -1246,6 +1268,64 @@ int sdlog2_thread_main(int argc, char *argv[]) LOGBUFFER_WRITE_AND_COUNT(SENS); } + if (buf.sensor.accelerometer1_timestamp != accelerometer1_timestamp) { + accelerometer1_timestamp = buf.sensor.accelerometer1_timestamp; + write_IMU1 = true; + } + + if (buf.sensor.gyro1_timestamp != gyro1_timestamp) { + gyro1_timestamp = buf.sensor.gyro1_timestamp; + write_IMU1 = true; + } + + if (buf.sensor.magnetometer1_timestamp != magnetometer1_timestamp) { + magnetometer1_timestamp = buf.sensor.magnetometer1_timestamp; + write_IMU1 = true; + } + + if (write_IMU1) { + log_msg.msg_type = LOG_IMU1_MSG; + log_msg.body.log_IMU.gyro_x = buf.sensor.gyro1_rad_s[0]; + log_msg.body.log_IMU.gyro_y = buf.sensor.gyro1_rad_s[1]; + log_msg.body.log_IMU.gyro_z = buf.sensor.gyro1_rad_s[2]; + log_msg.body.log_IMU.acc_x = buf.sensor.accelerometer1_m_s2[0]; + log_msg.body.log_IMU.acc_y = buf.sensor.accelerometer1_m_s2[1]; + log_msg.body.log_IMU.acc_z = buf.sensor.accelerometer1_m_s2[2]; + log_msg.body.log_IMU.mag_x = buf.sensor.magnetometer1_ga[0]; + log_msg.body.log_IMU.mag_y = buf.sensor.magnetometer1_ga[1]; + log_msg.body.log_IMU.mag_z = buf.sensor.magnetometer1_ga[2]; + LOGBUFFER_WRITE_AND_COUNT(IMU); + } + + if (buf.sensor.accelerometer2_timestamp != accelerometer2_timestamp) { + accelerometer2_timestamp = buf.sensor.accelerometer2_timestamp; + write_IMU2 = true; + } + + if (buf.sensor.gyro2_timestamp != gyro2_timestamp) { + gyro2_timestamp = buf.sensor.gyro2_timestamp; + write_IMU2 = true; + } + + if (buf.sensor.magnetometer2_timestamp != magnetometer2_timestamp) { + magnetometer2_timestamp = buf.sensor.magnetometer2_timestamp; + write_IMU2 = true; + } + + if (write_IMU2) { + log_msg.msg_type = LOG_IMU2_MSG; + log_msg.body.log_IMU.gyro_x = buf.sensor.gyro2_rad_s[0]; + log_msg.body.log_IMU.gyro_y = buf.sensor.gyro2_rad_s[1]; + log_msg.body.log_IMU.gyro_z = buf.sensor.gyro2_rad_s[2]; + log_msg.body.log_IMU.acc_x = buf.sensor.accelerometer2_m_s2[0]; + log_msg.body.log_IMU.acc_y = buf.sensor.accelerometer2_m_s2[1]; + log_msg.body.log_IMU.acc_z = buf.sensor.accelerometer2_m_s2[2]; + log_msg.body.log_IMU.mag_x = buf.sensor.magnetometer2_ga[0]; + log_msg.body.log_IMU.mag_y = buf.sensor.magnetometer2_ga[1]; + log_msg.body.log_IMU.mag_z = buf.sensor.magnetometer2_ga[2]; + LOGBUFFER_WRITE_AND_COUNT(IMU); + } + } /* --- ATTITUDE --- */ @@ -1352,17 +1432,20 @@ int sdlog2_thread_main(int argc, char *argv[]) /* --- GLOBAL POSITION SETPOINT --- */ if (copy_if_updated(ORB_ID(position_setpoint_triplet), subs.triplet_sub, &buf.triplet)) { - log_msg.msg_type = LOG_GPSP_MSG; - log_msg.body.log_GPSP.nav_state = 0; /* TODO: Fix this */ - log_msg.body.log_GPSP.lat = (int32_t)(buf.triplet.current.lat * 1e7d); - log_msg.body.log_GPSP.lon = (int32_t)(buf.triplet.current.lon * 1e7d); - log_msg.body.log_GPSP.alt = buf.triplet.current.alt; - log_msg.body.log_GPSP.yaw = buf.triplet.current.yaw; - log_msg.body.log_GPSP.type = buf.triplet.current.type; - log_msg.body.log_GPSP.loiter_radius = buf.triplet.current.loiter_radius; - log_msg.body.log_GPSP.loiter_direction = buf.triplet.current.loiter_direction; - log_msg.body.log_GPSP.pitch_min = buf.triplet.current.pitch_min; - LOGBUFFER_WRITE_AND_COUNT(GPSP); + + if (buf.triplet.current.valid) { + log_msg.msg_type = LOG_GPSP_MSG; + log_msg.body.log_GPSP.nav_state = buf.triplet.nav_state; + log_msg.body.log_GPSP.lat = (int32_t)(buf.triplet.current.lat * 1e7d); + log_msg.body.log_GPSP.lon = (int32_t)(buf.triplet.current.lon * 1e7d); + log_msg.body.log_GPSP.alt = buf.triplet.current.alt; + log_msg.body.log_GPSP.yaw = buf.triplet.current.yaw; + log_msg.body.log_GPSP.type = buf.triplet.current.type; + log_msg.body.log_GPSP.loiter_radius = buf.triplet.current.loiter_radius; + log_msg.body.log_GPSP.loiter_direction = buf.triplet.current.loiter_direction; + log_msg.body.log_GPSP.pitch_min = buf.triplet.current.pitch_min; + LOGBUFFER_WRITE_AND_COUNT(GPSP); + } } /* --- VICON POSITION --- */ @@ -1467,16 +1550,19 @@ int sdlog2_thread_main(int argc, char *argv[]) } /* --- TELEMETRY --- */ - if (copy_if_updated(ORB_ID(telemetry_status), subs.telemetry_sub, &buf.telemetry)) { - log_msg.msg_type = LOG_TELE_MSG; - log_msg.body.log_TELE.rssi = buf.telemetry.rssi; - log_msg.body.log_TELE.remote_rssi = buf.telemetry.remote_rssi; - log_msg.body.log_TELE.noise = buf.telemetry.noise; - log_msg.body.log_TELE.remote_noise = buf.telemetry.remote_noise; - log_msg.body.log_TELE.rxerrors = buf.telemetry.rxerrors; - log_msg.body.log_TELE.fixed = buf.telemetry.fixed; - log_msg.body.log_TELE.txbuf = buf.telemetry.txbuf; - LOGBUFFER_WRITE_AND_COUNT(TELE); + for (int i = 0; i < TELEMETRY_STATUS_ORB_ID_NUM; i++) { + if (copy_if_updated(telemetry_status_orb_id[i], subs.telemetry_subs[i], &buf.telemetry)) { + log_msg.msg_type = LOG_TEL0_MSG + i; + log_msg.body.log_TEL.rssi = buf.telemetry.rssi; + log_msg.body.log_TEL.remote_rssi = buf.telemetry.remote_rssi; + log_msg.body.log_TEL.noise = buf.telemetry.noise; + log_msg.body.log_TEL.remote_noise = buf.telemetry.remote_noise; + log_msg.body.log_TEL.rxerrors = buf.telemetry.rxerrors; + log_msg.body.log_TEL.fixed = buf.telemetry.fixed; + log_msg.body.log_TEL.txbuf = buf.telemetry.txbuf; + log_msg.body.log_TEL.heartbeat_time = buf.telemetry.heartbeat_time; + LOGBUFFER_WRITE_AND_COUNT(TEL); + } } /* --- BOTTOM DISTANCE --- */ @@ -1511,13 +1597,12 @@ int sdlog2_thread_main(int argc, char *argv[]) if (copy_if_updated(ORB_ID(tecs_status), subs.tecs_status_sub, &buf.tecs_status)) { log_msg.msg_type = LOG_TECS_MSG; log_msg.body.log_TECS.altitudeSp = buf.tecs_status.altitudeSp; - log_msg.body.log_TECS.altitude = buf.tecs_status.altitude; + log_msg.body.log_TECS.altitudeFiltered = buf.tecs_status.altitude_filtered; log_msg.body.log_TECS.flightPathAngleSp = buf.tecs_status.flightPathAngleSp; log_msg.body.log_TECS.flightPathAngle = buf.tecs_status.flightPathAngle; log_msg.body.log_TECS.flightPathAngleFiltered = buf.tecs_status.flightPathAngleFiltered; log_msg.body.log_TECS.airspeedSp = buf.tecs_status.airspeedSp; - log_msg.body.log_TECS.airspeed = buf.tecs_status.airspeed; - log_msg.body.log_TECS.airspeedFiltered = buf.tecs_status.airspeedFiltered; + log_msg.body.log_TECS.airspeedFiltered = buf.tecs_status.airspeed_filtered; log_msg.body.log_TECS.airspeedDerivativeSp = buf.tecs_status.airspeedDerivativeSp; log_msg.body.log_TECS.airspeedDerivative = buf.tecs_status.airspeedDerivative; log_msg.body.log_TECS.totalEnergyRateSp = buf.tecs_status.totalEnergyRateSp; diff --git a/src/modules/sdlog2/sdlog2_format.h b/src/modules/sdlog2/sdlog2_format.h index dc5e6c8bd..aff0e3f48 100644 --- a/src/modules/sdlog2/sdlog2_format.h +++ b/src/modules/sdlog2/sdlog2_format.h @@ -91,6 +91,14 @@ struct log_format_s { .labels = _labels \ } +#define LOG_FORMAT_S(_name, _struct_name, _format, _labels) { \ + .type = LOG_##_name##_MSG, \ + .length = sizeof(struct log_##_struct_name##_s) + LOG_PACKET_HEADER_LEN, \ + .name = #_name, \ + .format = _format, \ + .labels = _labels \ + } + #define LOG_FORMAT_MSG 0x80 #define LOG_PACKET_SIZE(_name) LOG_PACKET_HEADER_LEN + sizeof(struct log_##_name##_s) diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 8c05e87c5..fb7609435 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -73,6 +73,8 @@ struct log_ATSP_s { /* --- IMU - IMU SENSORS --- */ #define LOG_IMU_MSG 4 +#define LOG_IMU1_MSG 22 +#define LOG_IMU2_MSG 23 struct log_IMU_s { float acc_x; float acc_y; @@ -276,19 +278,8 @@ struct log_DIST_s { uint8_t flags; }; -/* --- TELE - TELEMETRY STATUS --- */ -#define LOG_TELE_MSG 22 -struct log_TELE_s { - uint8_t rssi; - uint8_t remote_rssi; - uint8_t noise; - uint8_t remote_noise; - uint16_t rxerrors; - uint16_t fixed; - uint8_t txbuf; -}; +/* LOG IMU1 and IMU2 MSGs consume IDs 22 and 23 */ -// ID 23 available /* --- PWR - ONBOARD POWER SYSTEM --- */ #define LOG_PWR_MSG 24 @@ -342,12 +333,11 @@ struct log_GS1B_s { #define LOG_TECS_MSG 30 struct log_TECS_s { float altitudeSp; - float altitude; + float altitudeFiltered; float flightPathAngleSp; float flightPathAngle; float flightPathAngleFiltered; float airspeedSp; - float airspeed; float airspeedFiltered; float airspeedDerivativeSp; float airspeedDerivative; @@ -385,6 +375,23 @@ struct log_EST1_s { float s[16]; }; +/* --- TEL0..3 - TELEMETRY STATUS --- */ +#define LOG_TEL0_MSG 34 +#define LOG_TEL1_MSG 35 +#define LOG_TEL2_MSG 36 +#define LOG_TEL3_MSG 37 +struct log_TEL_s { + uint8_t rssi; + uint8_t remote_rssi; + uint8_t noise; + uint8_t remote_noise; + uint16_t rxerrors; + uint16_t fixed; + uint8_t txbuf; + uint64_t heartbeat_time; +}; + + /********** SYSTEM MESSAGES, ID > 0x80 **********/ /* --- TIME - TIME STAMP --- */ @@ -414,7 +421,9 @@ static const struct log_format_s log_formats[] = { /* business-level messages, ID < 0x80 */ LOG_FORMAT(ATT, "fffffffff", "Roll,Pitch,Yaw,RollRate,PitchRate,YawRate,GX,GY,GZ"), LOG_FORMAT(ATSP, "ffff", "RollSP,PitchSP,YawSP,ThrustSP"), - LOG_FORMAT(IMU, "fffffffff", "AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ"), + LOG_FORMAT_S(IMU, IMU, "fffffffff", "AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ"), + LOG_FORMAT_S(IMU1, IMU, "fffffffff", "AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ"), + LOG_FORMAT_S(IMU2, IMU, "fffffffff", "AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ"), LOG_FORMAT(SENS, "fffff", "BaroPres,BaroAlt,BaroTemp,DiffPres,DiffPresFilt"), LOG_FORMAT(LPOS, "ffffffffLLfBBBff", "X,Y,Z,Dist,DistR,VX,VY,VZ,RLat,RLon,RAlt,PFlg,LFlg,GFlg,EPH,EPV"), LOG_FORMAT(LPSP, "ffff", "X,Y,Z,Yaw"), @@ -432,7 +441,10 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(GVSP, "fff", "VX,VY,VZ"), LOG_FORMAT(BATT, "ffff", "V,VFilt,C,Discharged"), LOG_FORMAT(DIST, "ffB", "Bottom,BottomRate,Flags"), - LOG_FORMAT(TELE, "BBBBHHB", "RSSI,RemRSSI,Noise,RemNoise,RXErr,Fixed,TXBuf"), + LOG_FORMAT_S(TEL0, TEL, "BBBBHHBQ", "RSSI,RemRSSI,Noise,RemNoise,RXErr,Fixed,TXBuf,HbTime"), + LOG_FORMAT_S(TEL1, TEL, "BBBBHHBQ", "RSSI,RemRSSI,Noise,RemNoise,RXErr,Fixed,TXBuf,HbTime"), + LOG_FORMAT_S(TEL2, TEL, "BBBBHHBQ", "RSSI,RemRSSI,Noise,RemNoise,RXErr,Fixed,TXBuf,HbTime"), + LOG_FORMAT_S(TEL3, TEL, "BBBBHHBQ", "RSSI,RemRSSI,Noise,RemNoise,RXErr,Fixed,TXBuf,HbTime"), LOG_FORMAT(EST0, "ffffffffffffBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,nStat,fNaN,fHealth,fTOut"), LOG_FORMAT(EST1, "ffffffffffffffff", "s12,s13,s14,s15,s16,s17,s18,s19,s20,s21,s22,s23,s24,s25,s26,s27"), LOG_FORMAT(PWR, "fffBBBBB", "Periph5V,Servo5V,RSSI,UsbOk,BrickOk,ServoOk,PeriphOC,HipwrOC"), @@ -441,7 +453,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(GS0B, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"), LOG_FORMAT(GS1A, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"), LOG_FORMAT(GS1B, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"), - LOG_FORMAT(TECS, "ffffffffffffffB", "AltSP,Alt,FSP,F,FF,AsSP,As,AsF,AsDSP,AsD,TERSP,TER,EDRSP,EDR,M"), + LOG_FORMAT(TECS, "fffffffffffffB", "ASP,AF,FSP,F,FF,AsSP,AsF,AsDSP,AsD,TERSP,TER,EDRSP,EDR,M"), LOG_FORMAT(WIND, "ffff", "X,Y,CovX,CovY"), /* system-level messages, ID >= 0x80 */ diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index c8a3ec8f0..7ce6ef5ef 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -194,16 +194,25 @@ PARAM_DEFINE_FLOAT(SENS_ACC_ZSCALE, 1.0f); /** * Differential pressure sensor offset * + * The offset (zero-reading) in Pascal + * * @group Sensor Calibration */ PARAM_DEFINE_FLOAT(SENS_DPRES_OFF, 0.0f); /** - * Differential pressure sensor analog enabled + * Differential pressure sensor analog scaling + * + * Pick the appropriate scaling from the datasheet. + * this number defines the (linear) conversion from voltage + * to Pascal (pa). For the MPXV7002DP this is 1000. + * + * NOTE: If the sensor always registers zero, try switching + * the static and dynamic tubes. * * @group Sensor Calibration */ -PARAM_DEFINE_INT32(SENS_DPRES_ANA, 0); +PARAM_DEFINE_FLOAT(SENS_DPRES_ANSC, 0); /** @@ -283,6 +292,19 @@ PARAM_DEFINE_FLOAT(SENS_BOARD_Z_OFF, 0.0f); */ PARAM_DEFINE_INT32(SENS_EXT_MAG_ROT, 0); +/** +* Set usage of external magnetometer +* +* * Set to 0 (default) to auto-detect (will try to get the external as primary) +* * Set to 1 to force the external magnetometer as primary +* * Set to 2 to force the internal magnetometer as primary +* +* @min 0 +* @max 2 +* @group Sensor Calibration +*/ +PARAM_DEFINE_INT32(SENS_EXT_MAG, 0); + /** * RC Channel 1 Minimum @@ -665,6 +687,15 @@ PARAM_DEFINE_INT32(RC_MAP_LOITER_SW, 0); PARAM_DEFINE_INT32(RC_MAP_ACRO_SW, 0); /** + * Offboard switch channel mapping. + * + * @min 0 + * @max 18 + * @group Radio Calibration + */ +PARAM_DEFINE_INT32(RC_MAP_OFFB_SW, 0); + +/** * Flaps channel mapping. * * @min 0 @@ -811,3 +842,20 @@ PARAM_DEFINE_FLOAT(RC_LOITER_TH, 0.5f); * */ PARAM_DEFINE_FLOAT(RC_ACRO_TH, 0.5f); + + +/** + * Threshold for selecting offboard mode + * + * min:-1 + * max:+1 + * + * 0-1 indicate where in the full channel range the threshold sits + * 0 : min + * 1 : max + * sign indicates polarity of comparison + * positive : true when channel>th + * negative : true when channel<th + * + */ +PARAM_DEFINE_FLOAT(RC_OFFB_TH, 0.5f); diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 3307354a0..4e8a8c01d 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -133,7 +133,7 @@ #endif #define BATT_V_LOWPASS 0.001f -#define BATT_V_IGNORE_THRESHOLD 3.5f +#define BATT_V_IGNORE_THRESHOLD 4.8f /** * HACK - true temperature is much less than indicated temperature in baro, @@ -199,9 +199,15 @@ private: bool _hil_enabled; /**< if true, HIL is active */ bool _publishing; /**< if true, we are publishing sensor data */ - int _gyro_sub; /**< raw gyro data subscription */ - int _accel_sub; /**< raw accel data subscription */ - int _mag_sub; /**< raw mag data subscription */ + int _gyro_sub; /**< raw gyro0 data subscription */ + int _accel_sub; /**< raw accel0 data subscription */ + int _mag_sub; /**< raw mag0 data subscription */ + int _gyro1_sub; /**< raw gyro1 data subscription */ + int _accel1_sub; /**< raw accel1 data subscription */ + int _mag1_sub; /**< raw mag1 data subscription */ + int _gyro2_sub; /**< raw gyro2 data subscription */ + int _accel2_sub; /**< raw accel2 data subscription */ + int _mag2_sub; /**< raw mag2 data subscription */ int _rc_sub; /**< raw rc channels data subscription */ int _baro_sub; /**< raw baro data subscription */ int _airspeed_sub; /**< airspeed subscription */ @@ -248,7 +254,7 @@ private: float accel_offset[3]; float accel_scale[3]; float diff_pres_offset_pa; - float diff_pres_analog_enabled; + float diff_pres_analog_scale; int board_rotation; int external_mag_rotation; @@ -266,6 +272,7 @@ private: int rc_map_posctl_sw; int rc_map_loiter_sw; int rc_map_acro_sw; + int rc_map_offboard_sw; int rc_map_flaps; @@ -282,12 +289,14 @@ private: float rc_return_th; float rc_loiter_th; float rc_acro_th; + float rc_offboard_th; bool rc_assist_inv; bool rc_auto_inv; bool rc_posctl_inv; bool rc_return_inv; bool rc_loiter_inv; bool rc_acro_inv; + bool rc_offboard_inv; float battery_voltage_scaling; float battery_current_scaling; @@ -308,7 +317,7 @@ private: param_t mag_offset[3]; param_t mag_scale[3]; param_t diff_pres_offset_pa; - param_t diff_pres_analog_enabled; + param_t diff_pres_analog_scale; param_t rc_map_roll; param_t rc_map_pitch; @@ -321,6 +330,7 @@ private: param_t rc_map_posctl_sw; param_t rc_map_loiter_sw; param_t rc_map_acro_sw; + param_t rc_map_offboard_sw; param_t rc_map_flaps; @@ -337,6 +347,7 @@ private: param_t rc_return_th; param_t rc_loiter_th; param_t rc_acro_th; + param_t rc_offboard_th; param_t battery_voltage_scaling; param_t battery_current_scaling; @@ -473,6 +484,12 @@ Sensors::Sensors() : _gyro_sub(-1), _accel_sub(-1), _mag_sub(-1), + _gyro1_sub(-1), + _accel1_sub(-1), + _mag1_sub(-1), + _gyro2_sub(-1), + _accel2_sub(-1), + _mag2_sub(-1), _rc_sub(-1), _baro_sub(-1), _vcontrol_mode_sub(-1), @@ -496,6 +513,7 @@ Sensors::Sensors() : _battery_current_timestamp(0) { memset(&_rc, 0, sizeof(_rc)); + memset(&_diff_pres, 0, sizeof(_diff_pres)); /* basic r/c parameters */ for (unsigned i = 0; i < _rc_max_chan_count; i++) { @@ -540,6 +558,7 @@ Sensors::Sensors() : _parameter_handles.rc_map_posctl_sw = param_find("RC_MAP_POSCTL_SW"); _parameter_handles.rc_map_loiter_sw = param_find("RC_MAP_LOITER_SW"); _parameter_handles.rc_map_acro_sw = param_find("RC_MAP_ACRO_SW"); + _parameter_handles.rc_map_offboard_sw = param_find("RC_MAP_OFFB_SW"); _parameter_handles.rc_map_aux1 = param_find("RC_MAP_AUX1"); _parameter_handles.rc_map_aux2 = param_find("RC_MAP_AUX2"); @@ -555,6 +574,7 @@ Sensors::Sensors() : _parameter_handles.rc_return_th = param_find("RC_RETURN_TH"); _parameter_handles.rc_loiter_th = param_find("RC_LOITER_TH"); _parameter_handles.rc_acro_th = param_find("RC_ACRO_TH"); + _parameter_handles.rc_offboard_th = param_find("RC_OFFB_TH"); /* gyro offsets */ _parameter_handles.gyro_offset[0] = param_find("SENS_GYRO_XOFF"); @@ -583,7 +603,7 @@ Sensors::Sensors() : /* Differential pressure offset */ _parameter_handles.diff_pres_offset_pa = param_find("SENS_DPRES_OFF"); - _parameter_handles.diff_pres_analog_enabled = param_find("SENS_DPRES_ANA"); + _parameter_handles.diff_pres_analog_scale = param_find("SENS_DPRES_ANSC"); _parameter_handles.battery_voltage_scaling = param_find("BAT_V_SCALING"); _parameter_handles.battery_current_scaling = param_find("BAT_C_SCALING"); @@ -707,6 +727,10 @@ Sensors::parameters_update() warnx("%s", paramerr); } + if (param_get(_parameter_handles.rc_map_offboard_sw, &(_parameters.rc_map_offboard_sw)) != OK) { + warnx("%s", paramerr); + } + if (param_get(_parameter_handles.rc_map_flaps, &(_parameters.rc_map_flaps)) != OK) { warnx("%s", paramerr); } @@ -735,6 +759,9 @@ Sensors::parameters_update() param_get(_parameter_handles.rc_acro_th, &(_parameters.rc_acro_th)); _parameters.rc_acro_inv = (_parameters.rc_acro_th < 0); _parameters.rc_acro_th = fabs(_parameters.rc_acro_th); + param_get(_parameter_handles.rc_offboard_th, &(_parameters.rc_offboard_th)); + _parameters.rc_offboard_inv = (_parameters.rc_offboard_th < 0); + _parameters.rc_offboard_th = fabs(_parameters.rc_offboard_th); /* update RC function mappings */ _rc.function[THROTTLE] = _parameters.rc_map_throttle - 1; @@ -747,6 +774,7 @@ Sensors::parameters_update() _rc.function[POSCTL] = _parameters.rc_map_posctl_sw - 1; _rc.function[LOITER] = _parameters.rc_map_loiter_sw - 1; _rc.function[ACRO] = _parameters.rc_map_acro_sw - 1; + _rc.function[OFFBOARD] = _parameters.rc_map_offboard_sw - 1; _rc.function[FLAPS] = _parameters.rc_map_flaps - 1; @@ -783,7 +811,7 @@ Sensors::parameters_update() /* Airspeed offset */ param_get(_parameter_handles.diff_pres_offset_pa, &(_parameters.diff_pres_offset_pa)); - param_get(_parameter_handles.diff_pres_analog_enabled, &(_parameters.diff_pres_analog_enabled)); + param_get(_parameter_handles.diff_pres_analog_scale, &(_parameters.diff_pres_analog_scale)); /* scaling of ADC ticks to battery voltage */ if (param_get(_parameter_handles.battery_voltage_scaling, &(_parameters.battery_voltage_scaling)) != OK) { @@ -988,7 +1016,7 @@ Sensors::accel_poll(struct sensor_combined_s &raw) if (accel_updated) { struct accel_report accel_report; - orb_copy(ORB_ID(sensor_accel), _accel_sub, &accel_report); + orb_copy(ORB_ID(sensor_accel0), _accel_sub, &accel_report); math::Vector<3> vect(accel_report.x, accel_report.y, accel_report.z); vect = _board_rotation * vect; @@ -1003,6 +1031,48 @@ Sensors::accel_poll(struct sensor_combined_s &raw) raw.accelerometer_timestamp = accel_report.timestamp; } + + orb_check(_accel1_sub, &accel_updated); + + if (accel_updated) { + struct accel_report accel_report; + + orb_copy(ORB_ID(sensor_accel1), _accel_sub, &accel_report); + + math::Vector<3> vect(accel_report.x, accel_report.y, accel_report.z); + vect = _board_rotation * vect; + + raw.accelerometer1_m_s2[0] = vect(0); + raw.accelerometer1_m_s2[1] = vect(1); + raw.accelerometer1_m_s2[2] = vect(2); + + raw.accelerometer1_raw[0] = accel_report.x_raw; + raw.accelerometer1_raw[1] = accel_report.y_raw; + raw.accelerometer1_raw[2] = accel_report.z_raw; + + raw.accelerometer1_timestamp = accel_report.timestamp; + } + + orb_check(_accel2_sub, &accel_updated); + + if (accel_updated) { + struct accel_report accel_report; + + orb_copy(ORB_ID(sensor_accel2), _accel_sub, &accel_report); + + math::Vector<3> vect(accel_report.x, accel_report.y, accel_report.z); + vect = _board_rotation * vect; + + raw.accelerometer2_m_s2[0] = vect(0); + raw.accelerometer2_m_s2[1] = vect(1); + raw.accelerometer2_m_s2[2] = vect(2); + + raw.accelerometer2_raw[0] = accel_report.x_raw; + raw.accelerometer2_raw[1] = accel_report.y_raw; + raw.accelerometer2_raw[2] = accel_report.z_raw; + + raw.accelerometer2_timestamp = accel_report.timestamp; + } } void @@ -1014,7 +1084,7 @@ Sensors::gyro_poll(struct sensor_combined_s &raw) if (gyro_updated) { struct gyro_report gyro_report; - orb_copy(ORB_ID(sensor_gyro), _gyro_sub, &gyro_report); + orb_copy(ORB_ID(sensor_gyro0), _gyro_sub, &gyro_report); math::Vector<3> vect(gyro_report.x, gyro_report.y, gyro_report.z); vect = _board_rotation * vect; @@ -1029,6 +1099,48 @@ Sensors::gyro_poll(struct sensor_combined_s &raw) raw.timestamp = gyro_report.timestamp; } + + orb_check(_gyro1_sub, &gyro_updated); + + if (gyro_updated) { + struct gyro_report gyro_report; + + orb_copy(ORB_ID(sensor_gyro1), _gyro1_sub, &gyro_report); + + math::Vector<3> vect(gyro_report.x, gyro_report.y, gyro_report.z); + vect = _board_rotation * vect; + + raw.gyro1_rad_s[0] = vect(0); + raw.gyro1_rad_s[1] = vect(1); + raw.gyro1_rad_s[2] = vect(2); + + raw.gyro1_raw[0] = gyro_report.x_raw; + raw.gyro1_raw[1] = gyro_report.y_raw; + raw.gyro1_raw[2] = gyro_report.z_raw; + + raw.gyro1_timestamp = gyro_report.timestamp; + } + + orb_check(_gyro2_sub, &gyro_updated); + + if (gyro_updated) { + struct gyro_report gyro_report; + + orb_copy(ORB_ID(sensor_gyro2), _gyro_sub, &gyro_report); + + math::Vector<3> vect(gyro_report.x, gyro_report.y, gyro_report.z); + vect = _board_rotation * vect; + + raw.gyro2_rad_s[0] = vect(0); + raw.gyro2_rad_s[1] = vect(1); + raw.gyro2_rad_s[2] = vect(2); + + raw.gyro2_raw[0] = gyro_report.x_raw; + raw.gyro2_raw[1] = gyro_report.y_raw; + raw.gyro2_raw[2] = gyro_report.z_raw; + + raw.gyro2_timestamp = gyro_report.timestamp; + } } void @@ -1040,10 +1152,12 @@ Sensors::mag_poll(struct sensor_combined_s &raw) if (mag_updated) { struct mag_report mag_report; - orb_copy(ORB_ID(sensor_mag), _mag_sub, &mag_report); + orb_copy(ORB_ID(sensor_mag0), _mag_sub, &mag_report); math::Vector<3> vect(mag_report.x, mag_report.y, mag_report.z); + // XXX we need device-id based handling here + if (_mag_is_external) { vect = _external_mag_rotation * vect; @@ -1071,7 +1185,7 @@ Sensors::baro_poll(struct sensor_combined_s &raw) if (baro_updated) { - orb_copy(ORB_ID(sensor_baro), _baro_sub, &_barometer); + orb_copy(ORB_ID(sensor_baro0), _baro_sub, &_barometer); raw.baro_pres_mbar = _barometer.pressure; // Pressure in mbar raw.baro_alt_meter = _barometer.altitude; // Altitude in meters @@ -1308,22 +1422,23 @@ Sensors::adc_poll(struct sensor_combined_s &raw) } else if (ADC_AIRSPEED_VOLTAGE_CHANNEL == buf_adc[i].am_channel) { /* calculate airspeed, raw is the difference from */ - float voltage = (float)(buf_adc[i].am_data) * 3.3f / 4096.0f * 2.0f; //V_ref/4096 * (voltage divider factor) + float voltage = (float)(buf_adc[i].am_data) * 3.3f / 4096.0f * 2.0f; // V_ref/4096 * (voltage divider factor) /** * The voltage divider pulls the signal down, only act on * a valid voltage from a connected sensor. Also assume a non- * zero offset from the sensor if its connected. */ - if (voltage > 0.4f && (_parameters.diff_pres_analog_enabled > 0)) { + if (voltage > 0.4f && (_parameters.diff_pres_analog_scale > 0.0f)) { - float diff_pres_pa = voltage * 1000.0f - _parameters.diff_pres_offset_pa; //for MPXV7002DP sensor + float diff_pres_pa_raw = voltage * _parameters.diff_pres_analog_scale - _parameters.diff_pres_offset_pa; + float diff_pres_pa = (diff_pres_pa_raw > 0.0f) ? diff_pres_pa_raw : 0.0f; _diff_pres.timestamp = t; _diff_pres.differential_pressure_pa = diff_pres_pa; - _diff_pres.differential_pressure_filtered_pa = diff_pres_pa; + _diff_pres.differential_pressure_raw_pa = diff_pres_pa_raw; + _diff_pres.differential_pressure_filtered_pa = (_diff_pres.differential_pressure_filtered_pa * 0.9f) + (diff_pres_pa * 0.1f); _diff_pres.temperature = -1000.0f; - _diff_pres.voltage = voltage; /* announce the airspeed if needed, just publish else */ if (_diff_pres_pub > 0) { @@ -1545,6 +1660,7 @@ Sensors::rc_poll() manual.return_switch = get_rc_sw2pos_position(RETURN, _parameters.rc_return_th, _parameters.rc_return_inv); manual.loiter_switch = get_rc_sw2pos_position(LOITER, _parameters.rc_loiter_th, _parameters.rc_loiter_inv); manual.acro_switch = get_rc_sw2pos_position(ACRO, _parameters.rc_acro_th, _parameters.rc_acro_inv); + manual.offboard_switch = get_rc_sw2pos_position(OFFBOARD, _parameters.rc_offboard_th, _parameters.rc_offboard_inv); /* publish manual_control_setpoint topic */ if (_manual_control_pub > 0) { @@ -1600,11 +1716,17 @@ Sensors::task_main() /* * do subscriptions */ - _gyro_sub = orb_subscribe(ORB_ID(sensor_gyro)); - _accel_sub = orb_subscribe(ORB_ID(sensor_accel)); - _mag_sub = orb_subscribe(ORB_ID(sensor_mag)); + _gyro_sub = orb_subscribe(ORB_ID(sensor_gyro0)); + _accel_sub = orb_subscribe(ORB_ID(sensor_accel0)); + _mag_sub = orb_subscribe(ORB_ID(sensor_mag0)); + _gyro1_sub = orb_subscribe(ORB_ID(sensor_gyro1)); + _accel1_sub = orb_subscribe(ORB_ID(sensor_accel1)); + _mag1_sub = orb_subscribe(ORB_ID(sensor_mag1)); + _gyro2_sub = orb_subscribe(ORB_ID(sensor_gyro2)); + _accel2_sub = orb_subscribe(ORB_ID(sensor_accel2)); + _mag2_sub = orb_subscribe(ORB_ID(sensor_mag2)); _rc_sub = orb_subscribe(ORB_ID(input_rc)); - _baro_sub = orb_subscribe(ORB_ID(sensor_baro)); + _baro_sub = orb_subscribe(ORB_ID(sensor_baro0)); _diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure)); _vcontrol_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); _params_sub = orb_subscribe(ORB_ID(parameter_update)); diff --git a/src/modules/systemlib/board_serial.c b/src/modules/systemlib/board_serial.c index ad8c2bf83..182fd15c6 100644 --- a/src/modules/systemlib/board_serial.c +++ b/src/modules/systemlib/board_serial.c @@ -44,11 +44,11 @@ #include "board_config.h" #include "board_serial.h" -int get_board_serial(char *serialid) +int get_board_serial(uint8_t *serialid) { - const volatile unsigned *udid_ptr = (const unsigned *)UDID_START; + const volatile uint32_t *udid_ptr = (const uint32_t *)UDID_START; union udid id; - val_read((unsigned *)&id, udid_ptr, sizeof(id)); + val_read((uint32_t *)&id, udid_ptr, sizeof(id)); /* Copy the serial from the chips non-write memory and swap endianess */ @@ -57,4 +57,4 @@ int get_board_serial(char *serialid) serialid[8] = id.data[11]; serialid[9] = id.data[10]; serialid[10] = id.data[9]; serialid[11] = id.data[8]; return 0; -}
\ No newline at end of file +} diff --git a/src/modules/systemlib/board_serial.h b/src/modules/systemlib/board_serial.h index b14bb4376..873d9657b 100644 --- a/src/modules/systemlib/board_serial.h +++ b/src/modules/systemlib/board_serial.h @@ -44,6 +44,6 @@ __BEGIN_DECLS -__EXPORT int get_board_serial(char *serialid); +__EXPORT int get_board_serial(uint8_t *serialid); __END_DECLS diff --git a/src/modules/systemlib/circuit_breaker.c b/src/modules/systemlib/circuit_breaker.c index b59531d69..64317a18a 100644 --- a/src/modules/systemlib/circuit_breaker.c +++ b/src/modules/systemlib/circuit_breaker.c @@ -83,10 +83,22 @@ PARAM_DEFINE_INT32(CBRK_RATE_CTRL, 0); */ PARAM_DEFINE_INT32(CBRK_IO_SAFETY, 0); +/** + * Circuit breaker for airspeed sensor + * + * Setting this parameter to 162128 will disable the check for an airspeed sensor. + * WARNING: ENABLING THIS CIRCUIT BREAKER IS AT OWN RISK + * + * @min 0 + * @max 162128 + * @group Circuit Breaker + */ +PARAM_DEFINE_INT32(CBRK_AIRSPD_CHK, 0); + bool circuit_breaker_enabled(const char* breaker, int32_t magic) { int32_t val; - (void)param_get(param_find(breaker), val); + (void)param_get(param_find(breaker), &val); return (val == magic); } diff --git a/src/modules/systemlib/circuit_breaker.h b/src/modules/systemlib/circuit_breaker.h index 86439e2a5..b60584608 100644 --- a/src/modules/systemlib/circuit_breaker.h +++ b/src/modules/systemlib/circuit_breaker.h @@ -40,9 +40,19 @@ #ifndef CIRCUIT_BREAKER_H_ #define CIRCUIT_BREAKER_H_ +/* SAFETY WARNING -- SAFETY WARNING -- SAFETY WARNING + * + * OBEY THE DOCUMENTATION FOR ALL CIRCUIT BREAKERS HERE, + * ENSURE TO READ CAREFULLY ALL SAFETY WARNINGS. + * http://pixhawk.org/dev/circuit_breakers + * + * CIRCUIT BREAKERS ARE NOT PART OF THE STANDARD OPERATION PROCEDURE + * AND MAY DISABLE CHECKS THAT ARE VITAL FOR SAFE FLIGHT. + */ #define CBRK_SUPPLY_CHK_KEY 894281 #define CBRK_RATE_CTRL_KEY 140253 #define CBRK_IO_SAFETY_KEY 22027 +#define CBRK_AIRSPD_CHK_KEY 162128 #include <stdbool.h> 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/err.c b/src/modules/systemlib/err.c index 6c0e876d1..998b5ac7d 100644 --- a/src/modules/systemlib/err.c +++ b/src/modules/systemlib/err.c @@ -86,7 +86,7 @@ warnerr_core(int errcode, const char *fmt, va_list args) fprintf(stderr, "\n"); #elif CONFIG_ARCH_LOWPUTC lowsyslog("%s: ", getprogname()); - lowvyslog(fmt, args); + lowvsyslog(fmt, args); /* convenience as many parts of NuttX use negative errno */ if (errcode < 0) diff --git a/src/modules/systemlib/mixer/mixer.h b/src/modules/systemlib/mixer/mixer.h index 225570fa4..cdf60febc 100644 --- a/src/modules/systemlib/mixer/mixer.h +++ b/src/modules/systemlib/mixer/mixer.h @@ -231,6 +231,10 @@ protected: static const char * skipline(const char *buf, unsigned &buflen); private: + + /* do not allow to copy due to prt data members */ + Mixer(const Mixer&); + Mixer& operator=(const Mixer&); }; /** @@ -307,6 +311,10 @@ public: private: Mixer *_first; /**< linked list of mixers */ + + /* do not allow to copy due to pointer data members */ + MixerGroup(const MixerGroup&); + MixerGroup operator=(const MixerGroup&); }; /** @@ -424,6 +432,10 @@ private: mixer_scaler_s &scaler, uint8_t &control_group, uint8_t &control_index); + + /* do not allow to copy due to ptr data members */ + SimpleMixer(const SimpleMixer&); + SimpleMixer operator=(const SimpleMixer&); }; /** @@ -522,6 +534,9 @@ private: unsigned _rotor_count; const Rotor *_rotors; + /* do not allow to copy due to ptr data members */ + MultirotorMixer(const MultirotorMixer&); + MultirotorMixer operator=(const MultirotorMixer&); }; #endif diff --git a/src/modules/systemlib/otp.h b/src/modules/systemlib/otp.h index f10e129d8..273b064f0 100644 --- a/src/modules/systemlib/otp.h +++ b/src/modules/systemlib/otp.h @@ -125,7 +125,7 @@ struct otp_lock { #pragma pack(push, 1) union udid { uint32_t serial[3]; - char data[12]; + uint8_t data[12]; }; #pragma pack(pop) 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/modules/uORB/Subscription.cpp b/src/modules/uORB/Subscription.cpp index c1d1a938f..44b6debc7 100644 --- a/src/modules/uORB/Subscription.cpp +++ b/src/modules/uORB/Subscription.cpp @@ -40,6 +40,7 @@ #include "topics/parameter_update.h" #include "topics/actuator_controls.h" #include "topics/vehicle_gps_position.h" +#include "topics/satellite_info.h" #include "topics/sensor_combined.h" #include "topics/vehicle_attitude.h" #include "topics/vehicle_global_position.h" @@ -88,6 +89,7 @@ T Subscription<T>::getData() { template class __EXPORT Subscription<parameter_update_s>; template class __EXPORT Subscription<actuator_controls_s>; template class __EXPORT Subscription<vehicle_gps_position_s>; +template class __EXPORT Subscription<satellite_info_s>; template class __EXPORT Subscription<sensor_combined_s>; template class __EXPORT Subscription<vehicle_attitude_s>; template class __EXPORT Subscription<vehicle_global_position_s>; diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index 7c3bb0009..08c3ce1ac 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -46,16 +46,23 @@ #include <drivers/drv_orb_dev.h> #include <drivers/drv_mag.h> -ORB_DEFINE(sensor_mag, struct mag_report); +ORB_DEFINE(sensor_mag0, struct mag_report); +ORB_DEFINE(sensor_mag1, struct mag_report); +ORB_DEFINE(sensor_mag2, struct mag_report); #include <drivers/drv_accel.h> -ORB_DEFINE(sensor_accel, struct accel_report); +ORB_DEFINE(sensor_accel0, struct accel_report); +ORB_DEFINE(sensor_accel1, struct accel_report); +ORB_DEFINE(sensor_accel2, struct accel_report); #include <drivers/drv_gyro.h> -ORB_DEFINE(sensor_gyro, struct gyro_report); +ORB_DEFINE(sensor_gyro0, struct gyro_report); +ORB_DEFINE(sensor_gyro1, struct gyro_report); +ORB_DEFINE(sensor_gyro2, struct gyro_report); #include <drivers/drv_baro.h> -ORB_DEFINE(sensor_baro, struct baro_report); +ORB_DEFINE(sensor_baro0, struct baro_report); +ORB_DEFINE(sensor_baro1, struct baro_report); #include <drivers/drv_range_finder.h> ORB_DEFINE(sensor_range_finder, struct range_finder_report); @@ -75,6 +82,9 @@ ORB_DEFINE(sensor_combined, struct sensor_combined_s); #include "topics/vehicle_gps_position.h" ORB_DEFINE(vehicle_gps_position, struct vehicle_gps_position_s); +#include "topics/satellite_info.h" +ORB_DEFINE(satellite_info, struct satellite_info_s); + #include "topics/home_position.h" ORB_DEFINE(home_position, struct home_position_s); @@ -183,7 +193,10 @@ ORB_DEFINE(actuator_outputs_2, struct actuator_outputs_s); ORB_DEFINE(actuator_outputs_3, struct actuator_outputs_s); #include "topics/telemetry_status.h" -ORB_DEFINE(telemetry_status, struct telemetry_status_s); +ORB_DEFINE(telemetry_status_0, struct telemetry_status_s); +ORB_DEFINE(telemetry_status_1, struct telemetry_status_s); +ORB_DEFINE(telemetry_status_2, struct telemetry_status_s); +ORB_DEFINE(telemetry_status_3, struct telemetry_status_s); #include "topics/debug_key_value.h" ORB_DEFINE(debug_key_value, struct debug_key_value_s); @@ -200,6 +213,9 @@ ORB_DEFINE(encoders, struct encoders_s); #include "topics/estimator_status.h" ORB_DEFINE(estimator_status, struct estimator_status_report); +#include "topics/vehicle_force_setpoint.h" +ORB_DEFINE(vehicle_force_setpoint, struct vehicle_force_setpoint_s); + #include "topics/tecs_status.h" ORB_DEFINE(tecs_status, struct tecs_status_s); diff --git a/src/modules/uORB/topics/actuator_armed.h b/src/modules/uORB/topics/actuator_armed.h index a98d3fc3a..0f6c9aca1 100644 --- a/src/modules/uORB/topics/actuator_armed.h +++ b/src/modules/uORB/topics/actuator_armed.h @@ -56,6 +56,7 @@ struct actuator_armed_s { bool armed; /**< Set to true if system is armed */ bool ready_to_arm; /**< Set to true if system is ready to be armed */ bool lockdown; /**< Set to true if actuators are forced to being disabled (due to emergency or HIL) */ + bool force_failsafe; /**< Set to true if the actuators are forced to the failsafe position */ }; /** @@ -65,4 +66,4 @@ struct actuator_armed_s { /* register this as object request broker structure */ ORB_DECLARE(actuator_armed); -#endif
\ No newline at end of file +#endif diff --git a/src/modules/uORB/topics/differential_pressure.h b/src/modules/uORB/topics/differential_pressure.h index 01e14cda9..cd48d3cb2 100644 --- a/src/modules/uORB/topics/differential_pressure.h +++ b/src/modules/uORB/topics/differential_pressure.h @@ -58,7 +58,6 @@ struct differential_pressure_s { float differential_pressure_raw_pa; /**< Raw differential pressure reading (may be negative) */ float differential_pressure_filtered_pa; /**< Low pass filtered differential pressure reading */ float max_differential_pressure_pa; /**< Maximum differential pressure reading */ - float voltage; /**< Voltage from analog airspeed sensors (voltage divider already compensated) */ float temperature; /**< Temperature provided by sensor, -1000.0f if unknown */ }; diff --git a/src/modules/uORB/topics/manual_control_setpoint.h b/src/modules/uORB/topics/manual_control_setpoint.h index 910b8a623..dde237adc 100644 --- a/src/modules/uORB/topics/manual_control_setpoint.h +++ b/src/modules/uORB/topics/manual_control_setpoint.h @@ -98,6 +98,7 @@ struct manual_control_setpoint_s { switch_pos_t posctl_switch; /**< position control 2 position switch (optional): _ALTCTL_, POSCTL */ switch_pos_t loiter_switch; /**< loiter 2 position switch (optional): _MISSION_, LOITER */ switch_pos_t acro_switch; /**< acro 2 position switch (optional): _MANUAL_, ACRO */ + switch_pos_t offboard_switch; /**< offboard 2 position switch (optional): _NORMAL_, OFFBOARD */ }; /**< manual control inputs */ /** diff --git a/src/modules/uORB/topics/mission.h b/src/modules/uORB/topics/mission.h index d9dd61df1..e4b72f87c 100644 --- a/src/modules/uORB/topics/mission.h +++ b/src/modules/uORB/topics/mission.h @@ -98,11 +98,15 @@ struct mission_item_s { unsigned do_jump_current_count; /**< count how many times the jump has been done */ }; +/** + * This topic used to notify navigator about mission changes, mission itself and new mission state + * must be stored in dataman before publication. + */ struct mission_s { - int dataman_id; /**< default -1, there are two offboard storage places in the dataman: 0 or 1 */ - unsigned count; /**< count of the missions stored in the datamanager */ - int current_index; /**< default -1, start at the one changed latest */ + int dataman_id; /**< default 0, there are two offboard storage places in the dataman: 0 or 1 */ + unsigned count; /**< count of the missions stored in the dataman */ + int current_seq; /**< default -1, start at the one changed latest */ }; /** diff --git a/src/modules/uORB/topics/mission_result.h b/src/modules/uORB/topics/mission_result.h index ad654a9ff..beb797e62 100644 --- a/src/modules/uORB/topics/mission_result.h +++ b/src/modules/uORB/topics/mission_result.h @@ -53,10 +53,10 @@ struct mission_result_s { - bool mission_reached; /**< true if mission has been reached */ - unsigned mission_index_reached; /**< index of the mission which has been reached */ - unsigned index_current_mission; /**< index of the current mission */ - bool mission_finished; /**< true if mission has been completed */ + unsigned seq_reached; /**< Sequence of the mission item which has been reached */ + unsigned seq_current; /**< Sequence of the current mission item */ + bool reached; /**< true if mission has been reached */ + bool finished; /**< true if mission has been completed */ }; /** diff --git a/src/modules/uORB/topics/offboard_control_setpoint.h b/src/modules/uORB/topics/offboard_control_setpoint.h index 68d3e494b..d7b131e3c 100644 --- a/src/modules/uORB/topics/offboard_control_setpoint.h +++ b/src/modules/uORB/topics/offboard_control_setpoint.h @@ -69,7 +69,7 @@ struct offboard_control_setpoint_s { uint64_t timestamp; enum OFFBOARD_CONTROL_MODE mode; /**< The current control inputs mode */ - bool armed; /**< Armed flag set, yes / no */ + float p1; /**< ailerons roll / roll rate input */ float p2; /**< elevator / pitch / pitch rate */ float p3; /**< rudder / yaw rate / yaw */ diff --git a/src/modules/uORB/topics/position_setpoint_triplet.h b/src/modules/uORB/topics/position_setpoint_triplet.h index ce42035ba..ec2131abd 100644 --- a/src/modules/uORB/topics/position_setpoint_triplet.h +++ b/src/modules/uORB/topics/position_setpoint_triplet.h @@ -60,16 +60,26 @@ enum SETPOINT_TYPE SETPOINT_TYPE_TAKEOFF, /**< takeoff setpoint */ SETPOINT_TYPE_LAND, /**< land setpoint, altitude must be ignored, descend until landing */ SETPOINT_TYPE_IDLE, /**< do nothing, switch off motors or keep at idle speed (MC) */ + SETPOINT_TYPE_OFFBOARD, /**< setpoint in NED frame (x, y, z, vx, vy, vz) set by offboard */ }; struct position_setpoint_s { bool valid; /**< true if setpoint is valid */ enum SETPOINT_TYPE type; /**< setpoint type to adjust behavior of position controller */ + float x; /**< local position setpoint in m in NED */ + float y; /**< local position setpoint in m in NED */ + float z; /**< local position setpoint in m in NED */ + bool position_valid; /**< true if local position setpoint valid */ + float vx; /**< local velocity setpoint in m/s in NED */ + float vy; /**< local velocity setpoint in m/s in NED */ + float vz; /**< local velocity setpoint in m/s in NED */ + bool velocity_valid; /**< true if local velocity setpoint valid */ double lat; /**< latitude, in deg */ double lon; /**< longitude, in deg */ float alt; /**< altitude AMSL, in m */ float yaw; /**< yaw (only for multirotors), in rad [-PI..PI), NaN = hold current yaw */ + float yawspeed; /**< yawspeed (only for multirotors, in rad/s) */ float loiter_radius; /**< loiter radius (only for fixed wing), in m */ int8_t loiter_direction; /**< loiter direction: 1 = CW, -1 = CCW */ float pitch_min; /**< minimal pitch angle for fixed wing takeoff waypoints */ @@ -85,6 +95,8 @@ struct position_setpoint_triplet_s struct position_setpoint_s previous; struct position_setpoint_s current; struct position_setpoint_s next; + + unsigned nav_state; /**< report the navigation state */ }; /** diff --git a/src/modules/uORB/topics/rc_channels.h b/src/modules/uORB/topics/rc_channels.h index 829d8e57d..8978de471 100644 --- a/src/modules/uORB/topics/rc_channels.h +++ b/src/modules/uORB/topics/rc_channels.h @@ -56,7 +56,7 @@ enum RC_CHANNELS_FUNCTION { RETURN, POSCTL, LOITER, - OFFBOARD_MODE, + OFFBOARD, ACRO, FLAPS, AUX_1, diff --git a/src/modules/uORB/topics/satellite_info.h b/src/modules/uORB/topics/satellite_info.h new file mode 100644 index 000000000..37c2faa96 --- /dev/null +++ b/src/modules/uORB/topics/satellite_info.h @@ -0,0 +1,89 @@ +/**************************************************************************** + * + * Copyright (C) 2014 PX4 Development Team. All rights reserved. + * Author: @author Thomas Gubler <thomasgubler@student.ethz.ch> + * @author Julian Oes <joes@student.ethz.ch> + * @author Lorenz Meier <lm@inf.ethz.ch> + * + * 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 satellite_info.h + * Definition of the GNSS satellite info uORB topic. + */ + +#ifndef TOPIC_SAT_INFO_H_ +#define TOPIC_SAT_INFO_H_ + +#include <stdint.h> +#include "../uORB.h" + +/** + * @addtogroup topics + * @{ + */ + +/** + * GNSS Satellite Info. + */ + +#define SAT_INFO_MAX_SATELLITES 20 + +struct satellite_info_s { + uint64_t timestamp; /**< Timestamp of satellite info */ + uint8_t count; /**< Number of satellites in satellite info */ + uint8_t svid[SAT_INFO_MAX_SATELLITES]; /**< Space vehicle ID [1..255], see scheme below */ + uint8_t used[SAT_INFO_MAX_SATELLITES]; /**< 0: Satellite not used, 1: used for navigation */ + uint8_t elevation[SAT_INFO_MAX_SATELLITES]; /**< Elevation (0: right on top of receiver, 90: on the horizon) of satellite */ + uint8_t azimuth[SAT_INFO_MAX_SATELLITES]; /**< Direction of satellite, 0: 0 deg, 255: 360 deg. */ + uint8_t snr[SAT_INFO_MAX_SATELLITES]; /**< dBHz, Signal to noise ratio of satellite C/N0, range 0..99, zero when not tracking this satellite. */ +}; + +/** + * NAV_SVINFO space vehicle ID (svid) scheme according to u-blox protocol specs + * u-bloxM8-V15_ReceiverDescriptionProtocolSpec_Public_(UBX-13003221).pdf + * + * GPS 1-32 + * SBAS 120-158 + * Galileo 211-246 + * BeiDou 159-163, 33-64 + * QZSS 193-197 + * GLONASS 65-96, 255 + * + */ + +/** + * @} + */ + +/* register this as object request broker structure */ +ORB_DECLARE(satellite_info); + +#endif diff --git a/src/modules/uORB/topics/sensor_combined.h b/src/modules/uORB/topics/sensor_combined.h index fa3367de9..06dfcdab3 100644 --- a/src/modules/uORB/topics/sensor_combined.h +++ b/src/modules/uORB/topics/sensor_combined.h @@ -95,6 +95,30 @@ struct sensor_combined_s { float magnetometer_cuttoff_freq_hz; /**< Internal analog low pass frequency of sensor */ uint64_t magnetometer_timestamp; /**< Magnetometer timestamp */ + int16_t gyro1_raw[3]; /**< Raw sensor values of angular velocity */ + float gyro1_rad_s[3]; /**< Angular velocity in radian per seconds */ + uint64_t gyro1_timestamp; /**< Gyro timestamp */ + + int16_t accelerometer1_raw[3]; /**< Raw acceleration in NED body frame */ + float accelerometer1_m_s2[3]; /**< Acceleration in NED body frame, in m/s^2 */ + uint64_t accelerometer1_timestamp; /**< Accelerometer timestamp */ + + int16_t magnetometer1_raw[3]; /**< Raw magnetic field in NED body frame */ + float magnetometer1_ga[3]; /**< Magnetic field in NED body frame, in Gauss */ + uint64_t magnetometer1_timestamp; /**< Magnetometer timestamp */ + + int16_t gyro2_raw[3]; /**< Raw sensor values of angular velocity */ + float gyro2_rad_s[3]; /**< Angular velocity in radian per seconds */ + uint64_t gyro2_timestamp; /**< Gyro timestamp */ + + int16_t accelerometer2_raw[3]; /**< Raw acceleration in NED body frame */ + float accelerometer2_m_s2[3]; /**< Acceleration in NED body frame, in m/s^2 */ + uint64_t accelerometer2_timestamp; /**< Accelerometer timestamp */ + + int16_t magnetometer2_raw[3]; /**< Raw magnetic field in NED body frame */ + float magnetometer2_ga[3]; /**< Magnetic field in NED body frame, in Gauss */ + uint64_t magnetometer2_timestamp; /**< Magnetometer timestamp */ + 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 */ diff --git a/src/modules/uORB/topics/tecs_status.h b/src/modules/uORB/topics/tecs_status.h index c4d0c1874..ddca19d61 100644 --- a/src/modules/uORB/topics/tecs_status.h +++ b/src/modules/uORB/topics/tecs_status.h @@ -51,11 +51,13 @@ */ typedef enum { - TECS_MODE_NORMAL, + TECS_MODE_NORMAL = 0, TECS_MODE_UNDERSPEED, TECS_MODE_TAKEOFF, TECS_MODE_LAND, - TECS_MODE_LAND_THROTTLELIM + TECS_MODE_LAND_THROTTLELIM, + TECS_MODE_BAD_DESCENT, + TECS_MODE_CLIMBOUT } tecs_mode; /** @@ -65,13 +67,12 @@ struct tecs_status_s { uint64_t timestamp; /**< timestamp, in microseconds since system start */ float altitudeSp; - float altitude; + float altitude_filtered; float flightPathAngleSp; float flightPathAngle; float flightPathAngleFiltered; float airspeedSp; - float airspeed; - float airspeedFiltered; + float airspeed_filtered; float airspeedDerivativeSp; float airspeedDerivative; diff --git a/src/modules/uORB/topics/telemetry_status.h b/src/modules/uORB/topics/telemetry_status.h index e9e00d76c..1297c1a9d 100644 --- a/src/modules/uORB/topics/telemetry_status.h +++ b/src/modules/uORB/topics/telemetry_status.h @@ -57,7 +57,8 @@ enum TELEMETRY_STATUS_RADIO_TYPE { struct telemetry_status_s { uint64_t timestamp; - uint64_t heartbeat_time; /**< Time of last received heartbeat from remote system */ + uint64_t heartbeat_time; /**< Time of last received heartbeat from remote system */ + uint64_t telem_time; /**< Time of last received telemetry status packet, 0 for none */ enum TELEMETRY_STATUS_RADIO_TYPE type; /**< type of the radio hardware */ uint8_t rssi; /**< local signal strength */ uint8_t remote_rssi; /**< remote signal strength */ @@ -72,6 +73,28 @@ struct telemetry_status_s { * @} */ -ORB_DECLARE(telemetry_status); +ORB_DECLARE(telemetry_status_0); +ORB_DECLARE(telemetry_status_1); +ORB_DECLARE(telemetry_status_2); +ORB_DECLARE(telemetry_status_3); + +#define TELEMETRY_STATUS_ORB_ID_NUM 4 + +static const struct orb_metadata *telemetry_status_orb_id[TELEMETRY_STATUS_ORB_ID_NUM] = { + ORB_ID(telemetry_status_0), + ORB_ID(telemetry_status_1), + ORB_ID(telemetry_status_2), + ORB_ID(telemetry_status_3), +}; + +// This is a hack to quiet an unused-variable warning for when telemetry_status.h is +// included but telemetry_status_orb_id is not referenced. The inline works if you +// choose to use it, but you can continue to just directly index into the array as well. +// If you don't use the inline this ends up being a no-op with no additional code emitted. +extern inline const struct orb_metadata *telemetry_status_orb_id_lookup(size_t index); +extern inline const struct orb_metadata *telemetry_status_orb_id_lookup(size_t index) +{ + return telemetry_status_orb_id[index]; +} #endif /* TOPIC_TELEMETRY_STATUS_H */ diff --git a/src/modules/uORB/topics/vehicle_command.h b/src/modules/uORB/topics/vehicle_command.h index c21a29b13..7db33d98b 100644 --- a/src/modules/uORB/topics/vehicle_command.h +++ b/src/modules/uORB/topics/vehicle_command.h @@ -76,6 +76,7 @@ enum VEHICLE_CMD { VEHICLE_CMD_DO_REPEAT_RELAY = 182, /* Cycle a relay on and off for a desired number of cyles with a desired period. |Relay number| Cycle count| Cycle time (seconds, decimal)| Empty| Empty| Empty| Empty| */ VEHICLE_CMD_DO_SET_SERVO = 183, /* Set a servo to a desired PWM value. |Servo number| PWM (microseconds, 1000 to 2000 typical)| Empty| Empty| Empty| Empty| Empty| */ VEHICLE_CMD_DO_REPEAT_SERVO = 184, /* Cycle a between its nominal setting and a desired PWM for a desired number of cycles with a desired period. |Servo number| PWM (microseconds, 1000 to 2000 typical)| Cycle count| Cycle time (seconds)| Empty| Empty| Empty| */ + VEHICLE_CMD_DO_FLIGHTTERMINATION=185, /* Terminate flight immediately |Flight termination activated if > 0.5| Empty| Empty| Empty| Empty| Empty| Empty| */ VEHICLE_CMD_DO_CONTROL_VIDEO = 200, /* Control onboard camera system. |Camera ID (-1 for all)| Transmission: 0: disabled, 1: enabled compressed, 2: enabled raw| Transmission mode: 0: video stream, >0: single images every n seconds (decimal)| Recording: 0: disabled, 1: enabled compressed, 2: enabled raw| Empty| Empty| Empty| */ VEHICLE_CMD_DO_LAST = 240, /* NOP - This command is only used to mark the upper limit of the DO commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */ VEHICLE_CMD_PREFLIGHT_CALIBRATION = 241, /* Trigger calibration. This command will be only accepted if in pre-flight mode. |Gyro calibration: 0: no, 1: yes| Magnetometer calibration: 0: no, 1: yes| Ground pressure: 0: no, 1: yes| Radio calibration: 0: no, 1: yes| Accelerometer calibration: 0: no, 1: yes| Empty| Empty| */ diff --git a/src/modules/uORB/topics/vehicle_control_mode.h b/src/modules/uORB/topics/vehicle_control_mode.h index ea554006f..49e2ba4b5 100644 --- a/src/modules/uORB/topics/vehicle_control_mode.h +++ b/src/modules/uORB/topics/vehicle_control_mode.h @@ -74,6 +74,7 @@ struct vehicle_control_mode_s { bool flag_control_manual_enabled; /**< true if manual input is mixed in */ bool flag_control_auto_enabled; /**< true if onboard autopilot should act */ + bool flag_control_offboard_enabled; /**< true if offboard control should be used */ bool flag_control_rates_enabled; /**< true if rates are stabilized */ bool flag_control_attitude_enabled; /**< true if attitude stabilization is mixed in */ bool flag_control_velocity_enabled; /**< true if horizontal velocity (implies direction) is controlled */ diff --git a/src/modules/mavlink/mavlink_commands.cpp b/src/modules/uORB/topics/vehicle_force_setpoint.h index fccd4d9a5..e3a7360b2 100644 --- a/src/modules/mavlink/mavlink_commands.cpp +++ b/src/modules/uORB/topics/vehicle_force_setpoint.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 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 @@ -32,39 +32,34 @@ ****************************************************************************/ /** - * @file mavlink_commands.cpp - * Mavlink commands stream implementation. - * - * @author Anton Babushkin <anton.babushkin@me.com> + * @file vehicle_force_setpoint.h + * @author Thomas Gubler <thomasgubler@gmail.com> + * Definition of force (NED) setpoint uORB topic. Typically this can be used + * by a position control app together with an attitude control app. + */ + +#ifndef TOPIC_VEHICLE_FORCE_SETPOINT_H_ +#define TOPIC_VEHICLE_FORCE_SETPOINT_H_ + +#include "../uORB.h" + +/** + * @addtogroup topics + * @{ */ -#include "mavlink_commands.h" +struct vehicle_force_setpoint_s { + float x; /**< in N NED */ + float y; /**< in N NED */ + float z; /**< in N NED */ + float yaw; /**< right-hand rotation around downward axis (rad, equivalent to Tait-Bryan yaw) */ +}; /**< Desired force in NED frame */ -MavlinkCommandsStream::MavlinkCommandsStream(Mavlink *mavlink, mavlink_channel_t channel) : _channel(channel), _cmd_time(0) -{ - _cmd_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_command)); -} +/** + * @} + */ -void -MavlinkCommandsStream::update(const hrt_abstime t) -{ - struct vehicle_command_s cmd; +/* register this as object request broker structure */ +ORB_DECLARE(vehicle_force_setpoint); - if (_cmd_sub->update(&_cmd_time, &cmd)) { - /* only send commands for other systems/components */ - if (cmd.target_system != mavlink_system.sysid || cmd.target_component != mavlink_system.compid) { - mavlink_msg_command_long_send(_channel, - cmd.target_system, - cmd.target_component, - cmd.command, - cmd.confirmation, - cmd.param1, - cmd.param2, - cmd.param3, - cmd.param4, - cmd.param5, - cmd.param6, - cmd.param7); - } - } -} +#endif diff --git a/src/modules/uORB/topics/vehicle_global_position.h b/src/modules/uORB/topics/vehicle_global_position.h index e32529cb4..25137c1c6 100644 --- a/src/modules/uORB/topics/vehicle_global_position.h +++ b/src/modules/uORB/topics/vehicle_global_position.h @@ -70,8 +70,8 @@ struct vehicle_global_position_s { float vel_e; /**< Ground east velocity, m/s */ float vel_d; /**< Ground downside velocity, m/s */ float yaw; /**< Yaw in radians -PI..+PI. */ - float eph; - float epv; + float eph; /**< Standard deviation of position estimate horizontally */ + float epv; /**< Standard deviation of position vertically */ }; /** diff --git a/src/modules/uORB/topics/vehicle_gps_position.h b/src/modules/uORB/topics/vehicle_gps_position.h index bbacb733a..80d65cd69 100644 --- a/src/modules/uORB/topics/vehicle_gps_position.h +++ b/src/modules/uORB/topics/vehicle_gps_position.h @@ -61,7 +61,6 @@ struct vehicle_gps_position_s { uint64_t timestamp_variance; float s_variance_m_s; /**< speed accuracy estimate m/s */ - float p_variance_m; /**< position accuracy estimate m */ float c_variance_rad; /**< course accuracy estimate rad */ uint8_t fix_type; /**< 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. */ @@ -82,14 +81,7 @@ struct vehicle_gps_position_s { uint64_t timestamp_time; /**< Timestamp for time information */ uint64_t time_gps_usec; /**< Timestamp (microseconds in GPS format), this is the timestamp which comes from the gps module */ - uint64_t timestamp_satellites; /**< Timestamp for sattelite information */ - uint8_t satellites_visible; /**< Number of satellites visible. If unknown, set to 255 */ - uint8_t satellite_prn[20]; /**< Global satellite ID */ - uint8_t satellite_used[20]; /**< 0: Satellite not used, 1: used for localization */ - uint8_t satellite_elevation[20]; /**< Elevation (0: right on top of receiver, 90: on the horizon) of satellite */ - uint8_t satellite_azimuth[20]; /**< Direction of satellite, 0: 0 deg, 255: 360 deg. */ - uint8_t satellite_snr[20]; /**< dBHz, Signal to noise ratio of satellite C/N0, range 0..99, zero when not tracking this satellite. */ - bool satellite_info_available; /**< 0 for no info, 1 for info available */ + uint8_t satellites_used; /**< Number of satellites used */ }; /** diff --git a/src/modules/uORB/topics/vehicle_local_position_setpoint.h b/src/modules/uORB/topics/vehicle_local_position_setpoint.h index 8988a0330..6766bb58a 100644 --- a/src/modules/uORB/topics/vehicle_local_position_setpoint.h +++ b/src/modules/uORB/topics/vehicle_local_position_setpoint.h @@ -50,11 +50,12 @@ */ struct vehicle_local_position_setpoint_s { + uint64_t timestamp; /**< timestamp of the setpoint */ float x; /**< in meters NED */ float y; /**< in meters NED */ float z; /**< in meters NED */ float yaw; /**< in radians NED -PI..+PI */ -}; /**< Local position in NED frame to go to */ +}; /**< Local position in NED frame */ /** * @} diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index 56590047f..b683bf98a 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -70,7 +70,8 @@ typedef enum { MAIN_STATE_AUTO_LOITER, MAIN_STATE_AUTO_RTL, MAIN_STATE_ACRO, - MAIN_STATE_MAX, + MAIN_STATE_OFFBOARD, + MAIN_STATE_MAX } main_state_t; // If you change the order, add or remove arming_state_t states make sure to update the arrays @@ -106,6 +107,7 @@ typedef enum { NAVIGATION_STATE_LAND, /**< Land mode */ NAVIGATION_STATE_DESCEND, /**< Descend mode (no position control) */ NAVIGATION_STATE_TERMINATION, /**< Termination mode */ + NAVIGATION_STATE_OFFBOARD, NAVIGATION_STATE_MAX, } navigation_state_t; @@ -184,7 +186,7 @@ struct vehicle_status_s { bool condition_system_sensors_initialized; bool condition_system_returned_to_home; bool condition_auto_mission_available; - bool condition_global_position_valid; /**< set to true by the commander app if the quality of the gps signal is good enough to use it in the position estimator */ + bool condition_global_position_valid; /**< set to true by the commander app if the quality of the position estimate is good enough to use it for navigation */ bool condition_launch_position_valid; /**< indicates a valid launch position */ bool condition_home_position_valid; /**< indicates a valid home position (a valid home position is not always a valid launch) */ bool condition_local_position_valid; @@ -224,6 +226,7 @@ struct vehicle_status_s { uint16_t errors_count4; bool circuit_breaker_engaged_power_check; + bool circuit_breaker_engaged_airspd_check; }; /** diff --git a/src/modules/uavcan/.gitignore b/src/modules/uavcan/.gitignore new file mode 100644 index 000000000..24fbf171f --- /dev/null +++ b/src/modules/uavcan/.gitignore @@ -0,0 +1 @@ +./dsdlc_generated/ diff --git a/src/modules/uavcan/esc_controller.cpp b/src/modules/uavcan/esc_controller.cpp new file mode 100644 index 000000000..406eba88c --- /dev/null +++ b/src/modules/uavcan/esc_controller.cpp @@ -0,0 +1,138 @@ +/**************************************************************************** + * + * 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 esc_controller.cpp + * + * @author Pavel Kirienko <pavel.kirienko@gmail.com> + */ + +#include "esc_controller.hpp" +#include <systemlib/err.h> + +UavcanEscController::UavcanEscController(uavcan::INode &node) : + _node(node), + _uavcan_pub_raw_cmd(node), + _uavcan_sub_status(node), + _orb_timer(node) +{ +} + +UavcanEscController::~UavcanEscController() +{ + perf_free(_perfcnt_invalid_input); + perf_free(_perfcnt_scaling_error); +} + +int UavcanEscController::init() +{ + // ESC status subscription + int res = _uavcan_sub_status.start(StatusCbBinder(this, &UavcanEscController::esc_status_sub_cb)); + if (res < 0) + { + warnx("ESC status sub failed %i", res); + return res; + } + + // ESC status will be relayed from UAVCAN bus into ORB at this rate + _orb_timer.setCallback(TimerCbBinder(this, &UavcanEscController::orb_pub_timer_cb)); + _orb_timer.startPeriodic(uavcan::MonotonicDuration::fromMSec(1000 / ESC_STATUS_UPDATE_RATE_HZ)); + + return res; +} + +void UavcanEscController::update_outputs(float *outputs, unsigned num_outputs) +{ + if ((outputs == nullptr) || (num_outputs > MAX_ESCS)) { + perf_count(_perfcnt_invalid_input); + return; + } + + /* + * Rate limiting - we don't want to congest the bus + */ + const auto timestamp = _node.getMonotonicTime(); + if ((timestamp - _prev_cmd_pub).toUSec() < (1000000 / MAX_RATE_HZ)) { + return; + } + _prev_cmd_pub = timestamp; + + /* + * Fill the command message + * If unarmed, we publish an empty message anyway + */ + uavcan::equipment::esc::RawCommand msg; + + if (_armed) { + for (unsigned i = 0; i < num_outputs; i++) { + + float scaled = (outputs[i] + 1.0F) * 0.5F * uavcan::equipment::esc::RawCommand::CMD_MAX; + if (scaled < 1.0F) { + scaled = 1.0F; // Since we're armed, we don't want to stop it completely + } + + if (scaled < uavcan::equipment::esc::RawCommand::CMD_MIN) { + scaled = uavcan::equipment::esc::RawCommand::CMD_MIN; + perf_count(_perfcnt_scaling_error); + } else if (scaled > uavcan::equipment::esc::RawCommand::CMD_MAX) { + scaled = uavcan::equipment::esc::RawCommand::CMD_MAX; + perf_count(_perfcnt_scaling_error); + } else { + ; // Correct value + } + + msg.cmd.push_back(static_cast<unsigned>(scaled)); + } + } + + /* + * Publish the command message to the bus + * Note that for a quadrotor it takes one CAN frame + */ + (void)_uavcan_pub_raw_cmd.broadcast(msg); +} + +void UavcanEscController::arm_esc(bool arm) +{ + _armed = arm; +} + +void UavcanEscController::esc_status_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::Status> &msg) +{ + // TODO save status into a local storage; publish to ORB later from orb_pub_timer_cb() +} + +void UavcanEscController::orb_pub_timer_cb(const uavcan::TimerEvent&) +{ + // TODO publish to ORB +} diff --git a/src/modules/uavcan/esc_controller.hpp b/src/modules/uavcan/esc_controller.hpp new file mode 100644 index 000000000..559ede561 --- /dev/null +++ b/src/modules/uavcan/esc_controller.hpp @@ -0,0 +1,107 @@ +/**************************************************************************** + * + * 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 esc_controller.hpp + * + * UAVCAN <--> ORB bridge for ESC messages: + * uavcan.equipment.esc.RawCommand + * uavcan.equipment.esc.RPMCommand + * uavcan.equipment.esc.Status + * + * @author Pavel Kirienko <pavel.kirienko@gmail.com> + */ + +#pragma once + +#include <uavcan/uavcan.hpp> +#include <uavcan/equipment/esc/RawCommand.hpp> +#include <uavcan/equipment/esc/Status.hpp> +#include <systemlib/perf_counter.h> + +class UavcanEscController +{ +public: + UavcanEscController(uavcan::INode& node); + ~UavcanEscController(); + + int init(); + + void update_outputs(float *outputs, unsigned num_outputs); + + void arm_esc(bool arm); + +private: + /** + * ESC status message reception will be reported via this callback. + */ + void esc_status_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::Status> &msg); + + /** + * ESC status will be published to ORB from this callback (fixed rate). + */ + void orb_pub_timer_cb(const uavcan::TimerEvent &event); + + + static constexpr unsigned MAX_RATE_HZ = 100; ///< XXX make this configurable + static constexpr unsigned ESC_STATUS_UPDATE_RATE_HZ = 5; + static constexpr unsigned MAX_ESCS = uavcan::equipment::esc::RawCommand::FieldTypes::cmd::MaxSize; + + typedef uavcan::MethodBinder<UavcanEscController*, + void (UavcanEscController::*)(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::Status>&)> + StatusCbBinder; + + typedef uavcan::MethodBinder<UavcanEscController*, void (UavcanEscController::*)(const uavcan::TimerEvent&)> + TimerCbBinder; + + /* + * libuavcan related things + */ + uavcan::MonotonicTime _prev_cmd_pub; ///< rate limiting + uavcan::INode &_node; + uavcan::Publisher<uavcan::equipment::esc::RawCommand> _uavcan_pub_raw_cmd; + uavcan::Subscriber<uavcan::equipment::esc::Status, StatusCbBinder> _uavcan_sub_status; + uavcan::TimerEventForwarder<TimerCbBinder> _orb_timer; + + /* + * ESC states + */ + bool _armed = false; + uavcan::equipment::esc::Status _states[MAX_ESCS]; + + /* + * Perf counters + */ + perf_counter_t _perfcnt_invalid_input = perf_alloc(PC_COUNT, "uavcan_esc_invalid_input"); + perf_counter_t _perfcnt_scaling_error = perf_alloc(PC_COUNT, "uavcan_esc_scaling_error"); +}; diff --git a/src/modules/uavcan/gnss_receiver.cpp b/src/modules/uavcan/gnss_receiver.cpp new file mode 100644 index 000000000..ba1fe5e49 --- /dev/null +++ b/src/modules/uavcan/gnss_receiver.cpp @@ -0,0 +1,153 @@ +/**************************************************************************** + * + * 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 gnss_receiver.cpp + * + * @author Pavel Kirienko <pavel.kirienko@gmail.com> + * @author Andrew Chambers <achamber@gmail.com> + * + */ + +#include "gnss_receiver.hpp" +#include <systemlib/err.h> +#include <mathlib/mathlib.h> + +#define MM_PER_CM 10 // Millimeters per centimeter + +UavcanGnssReceiver::UavcanGnssReceiver(uavcan::INode &node) : +_node(node), +_uavcan_sub_status(node), +_report_pub(-1) +{ +} + +int UavcanGnssReceiver::init() +{ + int res = -1; + + // GNSS fix subscription + res = _uavcan_sub_status.start(FixCbBinder(this, &UavcanGnssReceiver::gnss_fix_sub_cb)); + if (res < 0) + { + warnx("GNSS fix sub failed %i", res); + return res; + } + + // Clear the uORB GPS report + memset(&_report, 0, sizeof(_report)); + + return res; +} + +void UavcanGnssReceiver::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::gnss::Fix> &msg) +{ + _report.timestamp_position = hrt_absolute_time(); + _report.lat = msg.lat_1e7; + _report.lon = msg.lon_1e7; + _report.alt = msg.alt_1e2 * MM_PER_CM; // Convert from centimeter (1e2) to millimeters (1e3) + + _report.timestamp_variance = _report.timestamp_position; + + + // Check if the msg contains valid covariance information + const bool valid_position_covariance = !msg.position_covariance.empty(); + const bool valid_velocity_covariance = !msg.velocity_covariance.empty(); + + if (valid_position_covariance) { + float pos_cov[9]; + msg.position_covariance.unpackSquareMatrix(pos_cov); + + // Horizontal position uncertainty + const float horizontal_pos_variance = math::max(pos_cov[0], pos_cov[4]); + _report.eph = (horizontal_pos_variance > 0) ? sqrtf(horizontal_pos_variance) : -1.0F; + + // Vertical position uncertainty + _report.epv = (pos_cov[8] > 0) ? sqrtf(pos_cov[8]) : -1.0F; + } else { + _report.eph = -1.0F; + _report.epv = -1.0F; + } + + if (valid_velocity_covariance) { + float vel_cov[9]; + msg.velocity_covariance.unpackSquareMatrix(vel_cov); + _report.s_variance_m_s = math::max(math::max(vel_cov[0], vel_cov[4]), vel_cov[8]); + + /* There is a nonlinear relationship between the velocity vector and the heading. + * Use Jacobian to transform velocity covariance to heading covariance + * + * Nonlinear equation: + * heading = atan2(vel_e_m_s, vel_n_m_s) + * For math, see http://en.wikipedia.org/wiki/Atan2#Derivative + * + * To calculate the variance of heading from the variance of velocity, + * cov(heading) = J(velocity)*cov(velocity)*J(velocity)^T + */ + float vel_n = msg.ned_velocity[0]; + float vel_e = msg.ned_velocity[1]; + float vel_n_sq = vel_n * vel_n; + float vel_e_sq = vel_e * vel_e; + _report.c_variance_rad = + (vel_e_sq * vel_cov[0] + + -2 * vel_n * vel_e * vel_cov[1] + // Covariance matrix is symmetric + vel_n_sq* vel_cov[4]) / ((vel_n_sq + vel_e_sq) * (vel_n_sq + vel_e_sq)); + + } else { + _report.s_variance_m_s = -1.0F; + _report.c_variance_rad = -1.0F; + } + + _report.fix_type = msg.status; + + _report.timestamp_velocity = _report.timestamp_position; + _report.vel_n_m_s = msg.ned_velocity[0]; + _report.vel_e_m_s = msg.ned_velocity[1]; + _report.vel_d_m_s = msg.ned_velocity[2]; + _report.vel_m_s = sqrtf(_report.vel_n_m_s * _report.vel_n_m_s + _report.vel_e_m_s * _report.vel_e_m_s + _report.vel_d_m_s * _report.vel_d_m_s); + _report.cog_rad = atan2f(_report.vel_e_m_s, _report.vel_n_m_s); + _report.vel_ned_valid = true; + + _report.timestamp_time = _report.timestamp_position; + _report.time_gps_usec = uavcan::UtcTime(msg.gnss_timestamp).toUSec(); // Convert to microseconds + + _report.satellites_used = msg.sats_used; + + if (_report_pub > 0) { + orb_publish(ORB_ID(vehicle_gps_position), _report_pub, &_report); + + } else { + _report_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report); + } + +} diff --git a/src/modules/uavcan/gnss_receiver.hpp b/src/modules/uavcan/gnss_receiver.hpp new file mode 100644 index 000000000..18df8da2f --- /dev/null +++ b/src/modules/uavcan/gnss_receiver.hpp @@ -0,0 +1,84 @@ +/**************************************************************************** + * + * 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 gnss_receiver.hpp + * + * UAVCAN --> ORB bridge for GNSS messages: + * uavcan.equipment.gnss.Fix + * + * @author Pavel Kirienko <pavel.kirienko@gmail.com> + * @author Andrew Chambers <achamber@gmail.com> + */ + +#pragma once + +#include <drivers/drv_hrt.h> + +#include <uORB/uORB.h> +#include <uORB/topics/vehicle_gps_position.h> + +#include <uavcan/uavcan.hpp> +#include <uavcan/equipment/gnss/Fix.hpp> + +class UavcanGnssReceiver +{ +public: + UavcanGnssReceiver(uavcan::INode& node); + + int init(); + +private: + /** + * GNSS fix message will be reported via this callback. + */ + void gnss_fix_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::gnss::Fix> &msg); + + + typedef uavcan::MethodBinder<UavcanGnssReceiver*, + void (UavcanGnssReceiver::*)(const uavcan::ReceivedDataStructure<uavcan::equipment::gnss::Fix>&)> + FixCbBinder; + + /* + * libuavcan related things + */ + uavcan::INode &_node; + uavcan::Subscriber<uavcan::equipment::gnss::Fix, FixCbBinder> _uavcan_sub_status; + + /* + * uORB + */ + struct vehicle_gps_position_s _report; ///< uORB topic for gnss position + orb_advert_t _report_pub; ///< uORB pub for gnss position + +}; diff --git a/src/modules/uavcan/module.mk b/src/modules/uavcan/module.mk new file mode 100644 index 000000000..3865f2468 --- /dev/null +++ b/src/modules/uavcan/module.mk @@ -0,0 +1,70 @@ +############################################################################ +# +# Copyright (C) 2013 PX4 Development Team. All rights reserved. +# Author: Pavel Kirienko <pavel.kirienko@gmail.com> +# +# 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. +# +############################################################################ + +# +# UAVCAN <--> uORB bridge +# + +MODULE_COMMAND = uavcan + +MAXOPTIMIZATION = -Os + +SRCS += uavcan_main.cpp \ + uavcan_clock.cpp \ + esc_controller.cpp \ + gnss_receiver.cpp + +# +# libuavcan +# +include $(UAVCAN_DIR)/libuavcan/include.mk +SRCS += $(LIBUAVCAN_SRC) +INCLUDE_DIRS += $(LIBUAVCAN_INC) +# Since actual compiler mode is C++11, the library will default to UAVCAN_CPP11, but it will fail to compile +# because this platform lacks most of the standard library and STL. Hence we need to force C++03 mode. +override EXTRADEFINES := $(EXTRADEFINES) -DUAVCAN_CPP_VERSION=UAVCAN_CPP03 -DUAVCAN_NO_ASSERTIONS + +# +# libuavcan drivers for STM32 +# +include $(UAVCAN_DIR)/libuavcan_drivers/stm32/driver/include.mk +SRCS += $(LIBUAVCAN_STM32_SRC) +INCLUDE_DIRS += $(LIBUAVCAN_STM32_INC) +override EXTRADEFINES := $(EXTRADEFINES) -DUAVCAN_STM32_NUTTX -DUAVCAN_STM32_NUM_IFACES=2 + +# +# Invoke DSDL compiler +# +$(info $(shell $(LIBUAVCAN_DSDLC) $(UAVCAN_DSDL_DIR))) +INCLUDE_DIRS += dsdlc_generated diff --git a/src/modules/mavlink/mavlink_commands.h b/src/modules/uavcan/uavcan_clock.cpp index abdba34b9..e41d5f953 100644 --- a/src/modules/mavlink/mavlink_commands.h +++ b/src/modules/uavcan/uavcan_clock.cpp @@ -31,35 +31,49 @@ * ****************************************************************************/ +#include <uavcan_stm32/uavcan_stm32.hpp> +#include <drivers/drv_hrt.h> + /** - * @file mavlink_commands.h - * Mavlink commands stream definition. + * @file uavcan_clock.cpp + * + * Implements a clock for the CAN node. * - * @author Anton Babushkin <anton.babushkin@me.com> + * @author Pavel Kirienko <pavel.kirienko@gmail.com> */ -#ifndef MAVLINK_COMMANDS_H_ -#define MAVLINK_COMMANDS_H_ +namespace uavcan_stm32 +{ +namespace clock +{ + +uavcan::MonotonicTime getMonotonic() +{ + return uavcan::MonotonicTime::fromUSec(hrt_absolute_time()); +} -#include <uORB/uORB.h> -#include <uORB/topics/vehicle_command.h> +uavcan::UtcTime getUtc() +{ + return uavcan::UtcTime(); +} -class Mavlink; -class MavlinkCommansStream; +void adjustUtc(uavcan::UtcDuration adjustment) +{ + (void)adjustment; +} + +uavcan::uint64_t getUtcUSecFromCanInterrupt() +{ + return 0; +} -#include "mavlink_main.h" +} // namespace clock -class MavlinkCommandsStream +SystemClock &SystemClock::instance() { -private: - MavlinkOrbSubscription *_cmd_sub; - struct vehicle_command_s *_cmd; - mavlink_channel_t _channel; - uint64_t _cmd_time; + static SystemClock inst; + return inst; +} -public: - MavlinkCommandsStream(Mavlink *mavlink, mavlink_channel_t channel); - void update(const hrt_abstime t); -}; +} -#endif /* MAVLINK_COMMANDS_H_ */ diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp new file mode 100644 index 000000000..4535b6d6a --- /dev/null +++ b/src/modules/uavcan/uavcan_main.cpp @@ -0,0 +1,609 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#include <nuttx/config.h> + +#include <cstdlib> +#include <cstring> +#include <fcntl.h> +#include <systemlib/err.h> +#include <systemlib/systemlib.h> +#include <systemlib/mixer/mixer.h> +#include <systemlib/board_serial.h> +#include <version/version.h> +#include <arch/board/board.h> +#include <arch/chip/chip.h> + +#include <drivers/drv_hrt.h> +#include <drivers/drv_pwm_output.h> + +#include "uavcan_main.hpp" + +/** + * @file uavcan_main.cpp + * + * Implements basic functinality of UAVCAN node. + * + * @author Pavel Kirienko <pavel.kirienko@gmail.com> + */ + +/* + * UavcanNode + */ +UavcanNode *UavcanNode::_instance; + +UavcanNode::UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &system_clock) : + CDev("uavcan", UAVCAN_DEVICE_PATH), + _node(can_driver, system_clock), + _esc_controller(_node), + _gnss_receiver(_node) +{ + _control_topics[0] = ORB_ID(actuator_controls_0); + _control_topics[1] = ORB_ID(actuator_controls_1); + _control_topics[2] = ORB_ID(actuator_controls_2); + _control_topics[3] = ORB_ID(actuator_controls_3); + + // memset(_controls, 0, sizeof(_controls)); + // memset(_poll_fds, 0, sizeof(_poll_fds)); +} + +UavcanNode::~UavcanNode() +{ + if (_task != -1) { + /* tell the task we want it to go away */ + _task_should_exit = true; + + unsigned i = 10; + + do { + /* wait 5ms - it should wake every 10ms or so worst-case */ + usleep(5000); + + /* if we have given up, kill it */ + if (--i == 0) { + task_delete(_task); + break; + } + + } while (_task != -1); + } + + /* clean up the alternate device node */ + // unregister_driver(PWM_OUTPUT_DEVICE_PATH); + + ::close(_armed_sub); + + _instance = nullptr; +} + +int UavcanNode::start(uavcan::NodeID node_id, uint32_t bitrate) +{ + if (_instance != nullptr) { + warnx("Already started"); + return -1; + } + + /* + * GPIO config. + * Forced pull up on CAN2 is required for Pixhawk v1 where the second interface lacks a transceiver. + * If no transceiver is connected, the RX pin will float, occasionally causing CAN controller to + * fail during initialization. + */ + stm32_configgpio(GPIO_CAN1_RX); + stm32_configgpio(GPIO_CAN1_TX); + stm32_configgpio(GPIO_CAN2_RX | GPIO_PULLUP); + stm32_configgpio(GPIO_CAN2_TX); + + /* + * CAN driver init + */ + static CanInitHelper can; + static bool can_initialized = false; + + if (!can_initialized) { + const int can_init_res = can.init(bitrate); + + if (can_init_res < 0) { + warnx("CAN driver init failed %i", can_init_res); + return can_init_res; + } + + can_initialized = true; + } + + /* + * Node init + */ + _instance = new UavcanNode(can.driver, uavcan_stm32::SystemClock::instance()); + + if (_instance == nullptr) { + warnx("Out of memory"); + return -1; + } + + const int node_init_res = _instance->init(node_id); + + if (node_init_res < 0) { + delete _instance; + _instance = nullptr; + warnx("Node init failed %i", node_init_res); + return node_init_res; + } + + /* + * Start the task. Normally it should never exit. + */ + static auto run_trampoline = [](int, char *[]) {return UavcanNode::_instance->run();}; + _instance->_task = task_spawn_cmd("uavcan", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, StackSize, + static_cast<main_t>(run_trampoline), nullptr); + + if (_instance->_task < 0) { + warnx("start failed: %d", errno); + return -errno; + } + + return OK; +} + +void UavcanNode::fill_node_info() +{ + /* software version */ + uavcan::protocol::SoftwareVersion swver; + + // Extracting the first 8 hex digits of FW_GIT and converting them to int + char fw_git_short[9] = {}; + std::memmove(fw_git_short, FW_GIT, 8); + assert(fw_git_short[8] == '\0'); + char *end = nullptr; + swver.vcs_commit = std::strtol(fw_git_short, &end, 16); + swver.optional_field_mask |= swver.OPTIONAL_FIELD_MASK_VCS_COMMIT; + + warnx("SW version vcs_commit: 0x%08x", unsigned(swver.vcs_commit)); + + _node.setSoftwareVersion(swver); + + /* hardware version */ + uavcan::protocol::HardwareVersion hwver; + + if (!std::strncmp(HW_ARCH, "PX4FMU_V1", 9)) { + hwver.major = 1; + } else if (!std::strncmp(HW_ARCH, "PX4FMU_V2", 9)) { + hwver.major = 2; + } else { + ; // All other values of HW_ARCH resolve to zero + } + + uint8_t udid[12] = {}; // Someone seems to love magic numbers + get_board_serial(udid); + uavcan::copy(udid, udid + sizeof(udid), hwver.unique_id.begin()); + + _node.setHardwareVersion(hwver); +} + +int UavcanNode::init(uavcan::NodeID node_id) +{ + int ret = -1; + + /* do regular cdev init */ + ret = CDev::init(); + + if (ret != OK) + return ret; + + _node.setName("org.pixhawk.pixhawk"); + + _node.setNodeID(node_id); + + fill_node_info(); + + /* initializing the bridges UAVCAN <--> uORB */ + ret = _esc_controller.init(); + if (ret < 0) + return ret; + + ret = _gnss_receiver.init(); + if (ret < 0) + return ret; + + return _node.start(); +} + +void UavcanNode::node_spin_once() +{ + const int spin_res = _node.spin(uavcan::MonotonicTime()); + if (spin_res < 0) { + warnx("node spin error %i", spin_res); + } +} + +int UavcanNode::run() +{ + const unsigned PollTimeoutMs = 50; + + // XXX figure out the output count + _output_count = 2; + + _armed_sub = orb_subscribe(ORB_ID(actuator_armed)); + + actuator_outputs_s outputs; + memset(&outputs, 0, sizeof(outputs)); + + const int busevent_fd = ::open(uavcan_stm32::BusEvent::DevName, 0); + if (busevent_fd < 0) + { + warnx("Failed to open %s", uavcan_stm32::BusEvent::DevName); + _task_should_exit = true; + } + + /* + * XXX Mixing logic/subscriptions shall be moved into UavcanEscController::update(); + * IO multiplexing shall be done here. + */ + + _node.setStatusOk(); + + /* + * This event is needed to wake up the thread on CAN bus activity (RX/TX/Error). + * Please note that with such multiplexing it is no longer possible to rely only on + * the value returned from poll() to detect whether actuator control has timed out or not. + * Instead, all ORB events need to be checked individually (see below). + */ + _poll_fds_num = 0; + _poll_fds[_poll_fds_num] = ::pollfd(); + _poll_fds[_poll_fds_num].fd = busevent_fd; + _poll_fds[_poll_fds_num].events = POLLIN; + _poll_fds_num += 1; + + while (!_task_should_exit) { + // update actuator controls subscriptions if needed + if (_groups_subscribed != _groups_required) { + subscribe(); + _groups_subscribed = _groups_required; + } + + const int poll_ret = ::poll(_poll_fds, _poll_fds_num, PollTimeoutMs); + + node_spin_once(); // Non-blocking + + // this would be bad... + if (poll_ret < 0) { + log("poll error %d", errno); + continue; + } else { + // get controls for required topics + bool controls_updated = false; + unsigned poll_id = 1; + for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) { + if (_control_subs[i] > 0) { + if (_poll_fds[poll_id].revents & POLLIN) { + controls_updated = true; + orb_copy(_control_topics[i], _control_subs[i], &_controls[i]); + } + poll_id++; + } + } + + // can we mix? + if (controls_updated && (_mixers != nullptr)) { + + // XXX one output group has 8 outputs max, + // but this driver could well serve multiple groups. + unsigned num_outputs_max = 8; + + // Do mixing + outputs.noutputs = _mixers->mix(&outputs.output[0], num_outputs_max); + outputs.timestamp = hrt_absolute_time(); + + // iterate actuators + for (unsigned i = 0; i < outputs.noutputs; i++) { + // last resort: catch NaN, INF and out-of-band errors + if (!isfinite(outputs.output[i])) { + /* + * Value is NaN, INF or out of band - set to the minimum value. + * This will be clearly visible on the servo status and will limit the risk of accidentally + * spinning motors. It would be deadly in flight. + */ + outputs.output[i] = -1.0f; + } + + // limit outputs to valid range + + // never go below min + if (outputs.output[i] < -1.0f) { + outputs.output[i] = -1.0f; + } + + // never go below max + if (outputs.output[i] > 1.0f) { + outputs.output[i] = 1.0f; + } + } + + // Output to the bus + _esc_controller.update_outputs(outputs.output, outputs.noutputs); + } + + } + + // Check arming state + bool updated = false; + orb_check(_armed_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(actuator_armed), _armed_sub, &_armed); + + // Update the armed status and check that we're not locked down + bool set_armed = _armed.armed && !_armed.lockdown; + + arm_actuators(set_armed); + } + } + + teardown(); + warnx("exiting."); + + exit(0); +} + +int +UavcanNode::control_callback(uintptr_t handle, + uint8_t control_group, + uint8_t control_index, + float &input) +{ + const actuator_controls_s *controls = (actuator_controls_s *)handle; + + input = controls[control_group].control[control_index]; + return 0; +} + +int +UavcanNode::teardown() +{ + for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) { + if (_control_subs[i] > 0) { + ::close(_control_subs[i]); + _control_subs[i] = -1; + } + } + return ::close(_armed_sub); +} + +int +UavcanNode::arm_actuators(bool arm) +{ + _is_armed = arm; + _esc_controller.arm_esc(arm); + return OK; +} + +void +UavcanNode::subscribe() +{ + // Subscribe/unsubscribe to required actuator control groups + uint32_t sub_groups = _groups_required & ~_groups_subscribed; + uint32_t unsub_groups = _groups_subscribed & ~_groups_required; + // the first fd used by CAN + _poll_fds_num = 1; + for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) { + if (sub_groups & (1 << i)) { + warnx("subscribe to actuator_controls_%d", i); + _control_subs[i] = orb_subscribe(_control_topics[i]); + } + if (unsub_groups & (1 << i)) { + warnx("unsubscribe from actuator_controls_%d", i); + ::close(_control_subs[i]); + _control_subs[i] = -1; + } + + if (_control_subs[i] > 0) { + _poll_fds[_poll_fds_num].fd = _control_subs[i]; + _poll_fds[_poll_fds_num].events = POLLIN; + _poll_fds_num++; + } + } +} + +int +UavcanNode::ioctl(file *filp, int cmd, unsigned long arg) +{ + int ret = OK; + + lock(); + + switch (cmd) { + case PWM_SERVO_ARM: + arm_actuators(true); + break; + + case PWM_SERVO_SET_ARM_OK: + case PWM_SERVO_CLEAR_ARM_OK: + case PWM_SERVO_SET_FORCE_SAFETY_OFF: + // these are no-ops, as no safety switch + break; + + case PWM_SERVO_DISARM: + arm_actuators(false); + break; + + case MIXERIOCGETOUTPUTCOUNT: + *(unsigned *)arg = _output_count; + break; + + case MIXERIOCRESET: + if (_mixers != nullptr) { + delete _mixers; + _mixers = nullptr; + _groups_required = 0; + } + + break; + + case MIXERIOCLOADBUF: { + const char *buf = (const char *)arg; + unsigned buflen = strnlen(buf, 1024); + + if (_mixers == nullptr) + _mixers = new MixerGroup(control_callback, (uintptr_t)_controls); + + if (_mixers == nullptr) { + _groups_required = 0; + ret = -ENOMEM; + + } else { + + ret = _mixers->load_from_buf(buf, buflen); + + if (ret != 0) { + warnx("mixer load failed with %d", ret); + delete _mixers; + _mixers = nullptr; + _groups_required = 0; + ret = -EINVAL; + } else { + + _mixers->groups_required(_groups_required); + } + } + + break; + } + + default: + ret = -ENOTTY; + break; + } + + unlock(); + + if (ret == -ENOTTY) { + ret = CDev::ioctl(filp, cmd, arg); + } + + return ret; +} + +void +UavcanNode::print_info() +{ + if (!_instance) { + warnx("not running, start first"); + } + + warnx("actuators control groups: sub: %u / req: %u / fds: %u", (unsigned)_groups_subscribed, (unsigned)_groups_required, _poll_fds_num); + warnx("mixer: %s", (_mixers == nullptr) ? "NONE" : "OK"); +} + +/* + * App entry point + */ +static void print_usage() +{ + warnx("usage: uavcan start <node_id> [can_bitrate]"); +} + +extern "C" __EXPORT int uavcan_main(int argc, char *argv[]); + +int uavcan_main(int argc, char *argv[]) +{ + constexpr unsigned DEFAULT_CAN_BITRATE = 1000000; + + if (argc < 2) { + print_usage(); + ::exit(1); + } + + if (!std::strcmp(argv[1], "start")) { + if (argc < 3) { + print_usage(); + ::exit(1); + } + + /* + * Node ID + */ + const int node_id = atoi(argv[2]); + + if (node_id < 0 || node_id > uavcan::NodeID::Max || !uavcan::NodeID(node_id).isUnicast()) { + warnx("Invalid Node ID %i", node_id); + ::exit(1); + } + + /* + * CAN bitrate + */ + unsigned bitrate = 0; + + if (argc > 3) { + bitrate = atol(argv[3]); + } + + if (bitrate <= 0) { + bitrate = DEFAULT_CAN_BITRATE; + } + + if (UavcanNode::instance()) { + errx(1, "already started"); + } + + /* + * Start + */ + warnx("Node ID %u, bitrate %u", node_id, bitrate); + return UavcanNode::start(node_id, bitrate); + + } + + /* commands below require the app to be started */ + UavcanNode *inst = UavcanNode::instance(); + + if (!inst) { + errx(1, "application not running"); + } + + if (!std::strcmp(argv[1], "status") || !std::strcmp(argv[1], "info")) { + + inst->print_info(); + return OK; + } + + if (!std::strcmp(argv[1], "stop")) { + + delete inst; + inst = nullptr; + return OK; + } + + print_usage(); + ::exit(1); +} diff --git a/src/modules/uavcan/uavcan_main.hpp b/src/modules/uavcan/uavcan_main.hpp new file mode 100644 index 000000000..05b66fd7b --- /dev/null +++ b/src/modules/uavcan/uavcan_main.hpp @@ -0,0 +1,124 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#pragma once + +#include <nuttx/config.h> + +#include <uavcan_stm32/uavcan_stm32.hpp> +#include <drivers/device/device.h> + +#include <uORB/topics/actuator_controls.h> +#include <uORB/topics/actuator_outputs.h> +#include <uORB/topics/actuator_armed.h> + +#include "esc_controller.hpp" +#include "gnss_receiver.hpp" + +/** + * @file uavcan_main.hpp + * + * Defines basic functinality of UAVCAN node. + * + * @author Pavel Kirienko <pavel.kirienko@gmail.com> + */ + +#define NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN 4 +#define UAVCAN_DEVICE_PATH "/dev/uavcan/esc" + +/** + * A UAVCAN node. + */ +class UavcanNode : public device::CDev +{ + static constexpr unsigned MemPoolSize = 10752; + static constexpr unsigned RxQueueLenPerIface = 64; + static constexpr unsigned StackSize = 3000; + +public: + typedef uavcan::Node<MemPoolSize> Node; + typedef uavcan_stm32::CanInitHelper<RxQueueLenPerIface> CanInitHelper; + + UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &system_clock); + + virtual ~UavcanNode(); + + virtual int ioctl(file *filp, int cmd, unsigned long arg); + + static int start(uavcan::NodeID node_id, uint32_t bitrate); + + Node& getNode() { return _node; } + + static int control_callback(uintptr_t handle, + uint8_t control_group, + uint8_t control_index, + float &input); + + void subscribe(); + + int teardown(); + int arm_actuators(bool arm); + + void print_info(); + + static UavcanNode* instance() { return _instance; } + +private: + void fill_node_info(); + int init(uavcan::NodeID node_id); + void node_spin_once(); + int run(); + + int _task = -1; ///< handle to the OS task + bool _task_should_exit = false; ///< flag to indicate to tear down the CAN driver + int _armed_sub = -1; ///< uORB subscription of the arming status + actuator_armed_s _armed; ///< the arming request of the system + bool _is_armed = false; ///< the arming status of the actuators on the bus + + unsigned _output_count = 0; ///< number of actuators currently available + + static UavcanNode *_instance; ///< singleton pointer + Node _node; ///< library instance + UavcanEscController _esc_controller; + UavcanGnssReceiver _gnss_receiver; + + MixerGroup *_mixers = nullptr; + + uint32_t _groups_required = 0; + uint32_t _groups_subscribed = 0; + int _control_subs[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN] = {}; + actuator_controls_s _controls[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN] = {}; + orb_id_t _control_topics[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN] = {}; + pollfd _poll_fds[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN + 1] = {}; ///< +1 for /dev/uavcan/busevent + unsigned _poll_fds_num = 0; +}; |