From b98bbcb67835e119c3ceccedb5a91331b71397a0 Mon Sep 17 00:00:00 2001 From: Michelle Rossouw <64898873+MichelleRos@users.noreply.github.com> Date: Thu, 18 Mar 2021 14:12:54 +1100 Subject: [PATCH] Blimp: initial implementation of blimp vehicle type --- Blimp/AP_Arming.cpp | 488 +++++++++++++++++++++ Blimp/AP_Arming.h | 63 +++ Blimp/AP_State.cpp | 51 +++ Blimp/Blimp.cpp | 391 +++++++++++++++++ Blimp/Blimp.h | 544 +++++++++++++++++++++++ Blimp/Fins.cpp | 153 +++++++ Blimp/Fins.h | 175 ++++++++ Blimp/GCS_Blimp.cpp | 69 +++ Blimp/GCS_Blimp.h | 58 +++ Blimp/GCS_Mavlink.cpp | 977 ++++++++++++++++++++++++++++++++++++++++++ Blimp/GCS_Mavlink.h | 71 +++ Blimp/Log.cpp | 555 ++++++++++++++++++++++++ Blimp/Parameters.cpp | 727 +++++++++++++++++++++++++++++++ Blimp/Parameters.h | 529 +++++++++++++++++++++++ Blimp/RC_Channel.cpp | 255 +++++++++++ Blimp/RC_Channel.h | 50 +++ Blimp/commands.cpp | 110 +++++ Blimp/config.h | 593 +++++++++++++++++++++++++ Blimp/defines.h | 209 +++++++++ Blimp/ekf_check.cpp | 272 ++++++++++++ Blimp/events.cpp | 343 +++++++++++++++ Blimp/failsafe.cpp | 72 ++++ Blimp/inertia.cpp | 27 ++ Blimp/mode.cpp | 537 +++++++++++++++++++++++ Blimp/mode.h | 374 ++++++++++++++++ Blimp/mode_land.cpp | 24 ++ Blimp/mode_manual.cpp | 29 ++ Blimp/motors.cpp | 92 ++++ Blimp/radio.cpp | 192 +++++++++ Blimp/sensors.cpp | 62 +++ Blimp/system.cpp | 358 ++++++++++++++++ Blimp/version.h | 19 + Blimp/wscript | 31 ++ 33 files changed, 8500 insertions(+) create mode 100644 Blimp/AP_Arming.cpp create mode 100644 Blimp/AP_Arming.h create mode 100644 Blimp/AP_State.cpp create mode 100644 Blimp/Blimp.cpp create mode 100644 Blimp/Blimp.h create mode 100644 Blimp/Fins.cpp create mode 100644 Blimp/Fins.h create mode 100644 Blimp/GCS_Blimp.cpp create mode 100644 Blimp/GCS_Blimp.h create mode 100644 Blimp/GCS_Mavlink.cpp create mode 100644 Blimp/GCS_Mavlink.h create mode 100644 Blimp/Log.cpp create mode 100644 Blimp/Parameters.cpp create mode 100644 Blimp/Parameters.h create mode 100644 Blimp/RC_Channel.cpp create mode 100644 Blimp/RC_Channel.h create mode 100644 Blimp/commands.cpp create mode 100644 Blimp/config.h create mode 100644 Blimp/defines.h create mode 100644 Blimp/ekf_check.cpp create mode 100644 Blimp/events.cpp create mode 100644 Blimp/failsafe.cpp create mode 100644 Blimp/inertia.cpp create mode 100644 Blimp/mode.cpp create mode 100644 Blimp/mode.h create mode 100644 Blimp/mode_land.cpp create mode 100644 Blimp/mode_manual.cpp create mode 100644 Blimp/motors.cpp create mode 100644 Blimp/radio.cpp create mode 100644 Blimp/sensors.cpp create mode 100644 Blimp/system.cpp create mode 100644 Blimp/version.h create mode 100644 Blimp/wscript diff --git a/Blimp/AP_Arming.cpp b/Blimp/AP_Arming.cpp new file mode 100644 index 0000000000..f8505fbfdb --- /dev/null +++ b/Blimp/AP_Arming.cpp @@ -0,0 +1,488 @@ +#include "Blimp.h" + + +// performs pre-arm checks. expects to be called at 1hz. +void AP_Arming_Blimp::update(void) +{ + // perform pre-arm checks & display failures every 30 seconds + static uint8_t pre_arm_display_counter = PREARM_DISPLAY_PERIOD/2; + pre_arm_display_counter++; + bool display_fail = false; + if (pre_arm_display_counter >= PREARM_DISPLAY_PERIOD) { + display_fail = true; + pre_arm_display_counter = 0; + } + + pre_arm_checks(display_fail); +} + +bool AP_Arming_Blimp::pre_arm_checks(bool display_failure) +{ + const bool passed = run_pre_arm_checks(display_failure); + set_pre_arm_check(passed); + return passed; +} + +// perform pre-arm checks +// return true if the checks pass successfully +bool AP_Arming_Blimp::run_pre_arm_checks(bool display_failure) +{ + // exit immediately if already armed + if (blimp.motors->armed()) { + return true; + } + + // check if motor interlock and Emergency Stop aux switches are used + // at the same time. This cannot be allowed. + if (rc().find_channel_for_option(RC_Channel::AUX_FUNC::MOTOR_INTERLOCK) && + rc().find_channel_for_option(RC_Channel::AUX_FUNC::MOTOR_ESTOP)) { + check_failed(display_failure, "Interlock/E-Stop Conflict"); + return false; + } + + // check if motor interlock aux switch is in use + // if it is, switch needs to be in disabled position to arm + // otherwise exit immediately. This check to be repeated, + // as state can change at any time. + if (blimp.ap.using_interlock && blimp.ap.motor_interlock_switch) { + check_failed(display_failure, "Motor Interlock Enabled"); + } + + // if pre arm checks are disabled run only the mandatory checks + if (checks_to_perform == 0) { + return mandatory_checks(display_failure); + } + + return fence_checks(display_failure) + & parameter_checks(display_failure) + & motor_checks(display_failure) + & pilot_throttle_checks(display_failure) + & gcs_failsafe_check(display_failure) + & alt_checks(display_failure) + & AP_Arming::pre_arm_checks(display_failure); +} + +bool AP_Arming_Blimp::barometer_checks(bool display_failure) +{ + if (!AP_Arming::barometer_checks(display_failure)) { + return false; + } + + bool ret = true; + // check Baro + if ((checks_to_perform == ARMING_CHECK_ALL) || (checks_to_perform & ARMING_CHECK_BARO)) { + // Check baro & inav alt are within 1m if EKF is operating in an absolute position mode. + // Do not check if intending to operate in a ground relative height mode as EKF will output a ground relative height + // that may differ from the baro height due to baro drift. + nav_filter_status filt_status = blimp.inertial_nav.get_filter_status(); + bool using_baro_ref = (!filt_status.flags.pred_horiz_pos_rel && filt_status.flags.pred_horiz_pos_abs); + if (using_baro_ref) { + if (fabsf(blimp.inertial_nav.get_altitude() - blimp.baro_alt) > PREARM_MAX_ALT_DISPARITY_CM) { + check_failed(ARMING_CHECK_BARO, display_failure, "Altitude disparity"); + ret = false; + } + } + } + return ret; +} + +bool AP_Arming_Blimp::compass_checks(bool display_failure) +{ + bool ret = AP_Arming::compass_checks(display_failure); + + if ((checks_to_perform == ARMING_CHECK_ALL) || (checks_to_perform & ARMING_CHECK_COMPASS)) { + // check compass offsets have been set. AP_Arming only checks + // this if learning is off; Blimp *always* checks. + char failure_msg[50] = {}; + if (!AP::compass().configured(failure_msg, ARRAY_SIZE(failure_msg))) { + check_failed(ARMING_CHECK_COMPASS, display_failure, "%s", failure_msg); + ret = false; + } + } + + return ret; +} + +bool AP_Arming_Blimp::ins_checks(bool display_failure) +{ + bool ret = AP_Arming::ins_checks(display_failure); + + if ((checks_to_perform == ARMING_CHECK_ALL) || (checks_to_perform & ARMING_CHECK_INS)) { + + // get ekf attitude (if bad, it's usually the gyro biases) + if (!pre_arm_ekf_attitude_check()) { + check_failed(ARMING_CHECK_INS, display_failure, "EKF attitude is bad"); + ret = false; + } + } + + return ret; +} + +bool AP_Arming_Blimp::board_voltage_checks(bool display_failure) +{ + if (!AP_Arming::board_voltage_checks(display_failure)) { + return false; + } + + // check battery voltage + if ((checks_to_perform == ARMING_CHECK_ALL) || (checks_to_perform & ARMING_CHECK_VOLTAGE)) { + if (blimp.battery.has_failsafed()) { + check_failed(ARMING_CHECK_VOLTAGE, display_failure, "Battery failsafe"); + return false; + } + + // call parent battery checks + if (!AP_Arming::battery_checks(display_failure)) { + return false; + } + } + + return true; +} + +bool AP_Arming_Blimp::parameter_checks(bool display_failure) +{ + // check various parameter values + if ((checks_to_perform == ARMING_CHECK_ALL) || (checks_to_perform & ARMING_CHECK_PARAMETERS)) { + + // failsafe parameter checks + if (blimp.g.failsafe_throttle) { + // check throttle min is above throttle failsafe trigger and that the trigger is above ppm encoder's loss-of-signal value of 900 + if (blimp.channel_down->get_radio_min() <= blimp.g.failsafe_throttle_value+10 || blimp.g.failsafe_throttle_value < 910) { + check_failed(ARMING_CHECK_PARAMETERS, display_failure, "Check FS_THR_VALUE"); + return false; + } + } + if (blimp.g.failsafe_gcs == FS_GCS_ENABLED_CONTINUE_MISSION) { + // FS_GCS_ENABLE == 2 has been removed + check_failed(ARMING_CHECK_PARAMETERS, display_failure, "FS_GCS_ENABLE=2 removed, see FS_OPTIONS"); + } + + // lean angle parameter check + if (blimp.aparm.angle_max < 1000 || blimp.aparm.angle_max > 8000) { + check_failed(ARMING_CHECK_PARAMETERS, display_failure, "Check ANGLE_MAX"); + return false; + } + + // pilot-speed-up parameter check + if (blimp.g.pilot_speed_up <= 0) { + check_failed(ARMING_CHECK_PARAMETERS, display_failure, "Check PILOT_SPEED_UP"); + return false; + } + + // checks MOT_PWM_MIN/MAX for acceptable values + // if (!blimp.motors->check_mot_pwm_params()) { + // check_failed(ARMING_CHECK_PARAMETERS, display_failure, "Check MOT_PWM_MIN/MAX"); + // return false; + // } + + // ensure controllers are OK with us arming: + // char failure_msg[50]; + // if (!blimp.pos_control->pre_arm_checks("PSC", failure_msg, ARRAY_SIZE(failure_msg))) { + // check_failed(ARMING_CHECK_PARAMETERS, display_failure, "Bad parameter: %s", failure_msg); + // return false; + // } + // if (!blimp.attitude_control->pre_arm_checks("ATC", failure_msg, ARRAY_SIZE(failure_msg))) { + // check_failed(ARMING_CHECK_PARAMETERS, display_failure, "Bad parameter: %s", failure_msg); + // return false; + //} + } + + return true; +} + +// check motor setup was successful +bool AP_Arming_Blimp::motor_checks(bool display_failure) +{ + // check motors initialised correctly + if (!blimp.motors->initialised_ok()) { + check_failed(display_failure, "Check firmware or FRAME_CLASS"); + return false; + } + + // further checks enabled with parameters + if (!check_enabled(ARMING_CHECK_PARAMETERS)) { + return true; + } + + return true; +} + +bool AP_Arming_Blimp::pilot_throttle_checks(bool display_failure) +{ + // check throttle is above failsafe throttle + // this is near the bottom to allow other failures to be displayed before checking pilot throttle + if ((checks_to_perform == ARMING_CHECK_ALL) || (checks_to_perform & ARMING_CHECK_RC)) { + if (blimp.g.failsafe_throttle != FS_THR_DISABLED && blimp.channel_down->get_radio_in() < blimp.g.failsafe_throttle_value) { + const char *failmsg = "Throttle below Failsafe"; + check_failed(ARMING_CHECK_RC, display_failure, "%s", failmsg); + return false; + } + } + + return true; +} + +bool AP_Arming_Blimp::rc_calibration_checks(bool display_failure) +{ + return true; +} + +// performs pre_arm gps related checks and returns true if passed +bool AP_Arming_Blimp::gps_checks(bool display_failure) +{ + // run mandatory gps checks first + if (!mandatory_gps_checks(display_failure)) { + AP_Notify::flags.pre_arm_gps_check = false; + return false; + } + + // check if flight mode requires GPS + bool mode_requires_gps = blimp.flightmode->requires_GPS(); + + + // return true if GPS is not required + if (!mode_requires_gps) { + AP_Notify::flags.pre_arm_gps_check = true; + return true; + } + + // return true immediately if gps check is disabled + if (!(checks_to_perform == ARMING_CHECK_ALL || checks_to_perform & ARMING_CHECK_GPS)) { + AP_Notify::flags.pre_arm_gps_check = true; + return true; + } + + // warn about hdop separately - to prevent user confusion with no gps lock + if (blimp.gps.get_hdop() > blimp.g.gps_hdop_good) { + check_failed(ARMING_CHECK_GPS, display_failure, "High GPS HDOP"); + AP_Notify::flags.pre_arm_gps_check = false; + return false; + } + + // call parent gps checks + if (!AP_Arming::gps_checks(display_failure)) { + AP_Notify::flags.pre_arm_gps_check = false; + return false; + } + + // if we got here all must be ok + AP_Notify::flags.pre_arm_gps_check = true; + return true; +} + +// check ekf attitude is acceptable +bool AP_Arming_Blimp::pre_arm_ekf_attitude_check() +{ + // get ekf filter status + nav_filter_status filt_status = blimp.inertial_nav.get_filter_status(); + + return filt_status.flags.attitude; +} + +// performs mandatory gps checks. returns true if passed +bool AP_Arming_Blimp::mandatory_gps_checks(bool display_failure) +{ + // always check if inertial nav has started and is ready + const AP_AHRS_NavEKF &ahrs = AP::ahrs_navekf(); + char failure_msg[50] = {}; + if (!ahrs.pre_arm_check(false, failure_msg, sizeof(failure_msg))) { + check_failed(display_failure, "AHRS: %s", failure_msg); + return false; + } + + // check if flight mode requires GPS + bool mode_requires_gps = blimp.flightmode->requires_GPS(); + + if (mode_requires_gps) { + if (!blimp.position_ok()) { + // vehicle level position estimate checks + check_failed(display_failure, "Need Position Estimate"); + return false; + } + } else { + // return true if GPS is not required + return true; + } + + // if we got here all must be ok + return true; +} + +// Check GCS failsafe +bool AP_Arming_Blimp::gcs_failsafe_check(bool display_failure) +{ + if (blimp.failsafe.gcs) { + check_failed(display_failure, "GCS failsafe on"); + return false; + } + return true; +} + +// performs altitude checks. returns true if passed +bool AP_Arming_Blimp::alt_checks(bool display_failure) +{ + // always EKF altitude estimate + if (!blimp.flightmode->has_manual_throttle() && !blimp.ekf_alt_ok()) { + check_failed(display_failure, "Need Alt Estimate"); + return false; + } + + return true; +} + +// arm_checks - perform final checks before arming +// always called just before arming. Return true if ok to arm +// has side-effect that logging is started +bool AP_Arming_Blimp::arm_checks(AP_Arming::Method method) +{ + return AP_Arming::arm_checks(method); +} + +// mandatory checks that will be run if ARMING_CHECK is zero or arming forced +bool AP_Arming_Blimp::mandatory_checks(bool display_failure) +{ + // call mandatory gps checks and update notify status because regular gps checks will not run + bool result = mandatory_gps_checks(display_failure); + AP_Notify::flags.pre_arm_gps_check = result; + + // call mandatory alt check + if (!alt_checks(display_failure)) { + result = false; + } + + return result; +} + +void AP_Arming_Blimp::set_pre_arm_check(bool b) +{ + blimp.ap.pre_arm_check = b; + AP_Notify::flags.pre_arm_check = b; +} + +bool AP_Arming_Blimp::arm(const AP_Arming::Method method, const bool do_arming_checks) +{ + static bool in_arm_motors = false; + + // exit immediately if already in this function + if (in_arm_motors) { + return false; + } + in_arm_motors = true; + + // return true if already armed + if (blimp.motors->armed()) { + in_arm_motors = false; + return true; + } + + if (!AP_Arming::arm(method, do_arming_checks)) { + AP_Notify::events.arming_failed = true; + in_arm_motors = false; + return false; + } + + // let logger know that we're armed (it may open logs e.g.) + AP::logger().set_vehicle_armed(true); + + // notify that arming will occur (we do this early to give plenty of warning) + AP_Notify::flags.armed = true; + // call notify update a few times to ensure the message gets out + for (uint8_t i=0; i<=10; i++) { + AP::notify().update(); + } + + gcs().send_text(MAV_SEVERITY_INFO, "Arming motors"); //MIR kept in - usually only in SITL + + AP_AHRS_NavEKF &ahrs = AP::ahrs_navekf(); + + blimp.initial_armed_bearing = ahrs.yaw_sensor; + + if (!ahrs.home_is_set()) { + // Reset EKF altitude if home hasn't been set yet (we use EKF altitude as substitute for alt above home) + ahrs.resetHeightDatum(); + AP::logger().Write_Event(LogEvent::EKF_ALT_RESET); + + // we have reset height, so arming height is zero + blimp.arming_altitude_m = 0; + } else if (!ahrs.home_is_locked()) { + // Reset home position if it has already been set before (but not locked) + if (!blimp.set_home_to_current_location(false)) { + // ignore failure + } + + // remember the height when we armed + blimp.arming_altitude_m = blimp.inertial_nav.get_altitude() * 0.01; + } + + // enable gps velocity based centrefugal force compensation + ahrs.set_correct_centrifugal(true); + hal.util->set_soft_armed(true); + + // finally actually arm the motors + blimp.motors->armed(true); + + // log flight mode in case it was changed while vehicle was disarmed + AP::logger().Write_Mode((uint8_t)blimp.control_mode, blimp.control_mode_reason); + + // perf monitor ignores delay due to arming + AP::scheduler().perf_info.ignore_this_loop(); + + // flag exiting this function + in_arm_motors = false; + + // Log time stamp of arming event + blimp.arm_time_ms = millis(); + + // Start the arming delay + blimp.ap.in_arming_delay = true; + + // assumed armed without a arming, switch. Overridden in switches.cpp + blimp.ap.armed_with_switch = false; + + // return success + return true; +} + +// arming.disarm - disarm motors +bool AP_Arming_Blimp::disarm(const AP_Arming::Method method, bool do_disarm_checks) +{ + // return immediately if we are already disarmed + if (!blimp.motors->armed()) { + return true; + } + + if (!AP_Arming::disarm(method, do_disarm_checks)) { + return false; + } + + gcs().send_text(MAV_SEVERITY_INFO, "Disarming motors"); //MIR keeping in - usually only in SITL + + + AP_AHRS_NavEKF &ahrs = AP::ahrs_navekf(); + + // save compass offsets learned by the EKF if enabled + Compass &compass = AP::compass(); + if (ahrs.use_compass() && compass.get_learn_type() == Compass::LEARN_EKF) { + for (uint8_t i=0; iarmed(false); + + AP::logger().set_vehicle_armed(false); + + // disable gps velocity based centrefugal force compensation + ahrs.set_correct_centrifugal(false); + hal.util->set_soft_armed(false); + + blimp.ap.in_arming_delay = false; + + return true; +} diff --git a/Blimp/AP_Arming.h b/Blimp/AP_Arming.h new file mode 100644 index 0000000000..a6f5066a97 --- /dev/null +++ b/Blimp/AP_Arming.h @@ -0,0 +1,63 @@ +#pragma once + +#include + +class AP_Arming_Blimp : public AP_Arming +{ +public: + friend class Blimp; + friend class ToyMode; + + AP_Arming_Blimp() : AP_Arming() + { + // default REQUIRE parameter to 1 (Blimp does not have an + // actual ARMING_REQUIRE parameter) + require.set_default((uint8_t)Required::YES_MIN_PWM); + } + + /* Do not allow copies */ + AP_Arming_Blimp(const AP_Arming_Blimp &other) = delete; + AP_Arming_Blimp &operator=(const AP_Arming_Blimp&) = delete; + + void update(void); + + bool rc_calibration_checks(bool display_failure) override; + + bool disarm(AP_Arming::Method method, bool do_disarm_checks=true) override; + bool arm(AP_Arming::Method method, bool do_arming_checks=true) override; + +protected: + + bool pre_arm_checks(bool display_failure) override; + bool pre_arm_ekf_attitude_check(); + // bool proximity_checks(bool display_failure) const override; + bool arm_checks(AP_Arming::Method method) override; + + // mandatory checks that cannot be bypassed. This function will only be called if ARMING_CHECK is zero or arming forced + bool mandatory_checks(bool display_failure) override; + + // NOTE! the following check functions *DO* call into AP_Arming: + bool ins_checks(bool display_failure) override; + bool compass_checks(bool display_failure) override; + bool gps_checks(bool display_failure) override; + bool barometer_checks(bool display_failure) override; + bool board_voltage_checks(bool display_failure) override; + + // NOTE! the following check functions *DO NOT* call into AP_Arming! + bool parameter_checks(bool display_failure); + bool motor_checks(bool display_failure); + bool pilot_throttle_checks(bool display_failure); + bool oa_checks(bool display_failure); + bool mandatory_gps_checks(bool display_failure); + bool gcs_failsafe_check(bool display_failure); + bool alt_checks(bool display_failure); + + void set_pre_arm_check(bool b); + +private: + + // actually contains the pre-arm checks. This is wrapped so that + // we can store away success/failure of the checks. + bool run_pre_arm_checks(bool display_failure); + +}; diff --git a/Blimp/AP_State.cpp b/Blimp/AP_State.cpp new file mode 100644 index 0000000000..47214730d9 --- /dev/null +++ b/Blimp/AP_State.cpp @@ -0,0 +1,51 @@ +#include "Blimp.h" + +// --------------------------------------------- +void Blimp::set_auto_armed(bool b) +{ + // if no change, exit immediately + if ( ap.auto_armed == b ) { + return; + } + + ap.auto_armed = b; + if (b) { + AP::logger().Write_Event(LogEvent::AUTO_ARMED); + } +} + +// --------------------------------------------- +void Blimp::set_failsafe_radio(bool b) +{ + // only act on changes + // ------------------- + if (failsafe.radio != b) { + + // store the value so we don't trip the gate twice + // ----------------------------------------------- + failsafe.radio = b; + + if (failsafe.radio == false) { + // We've regained radio contact + // ---------------------------- + failsafe_radio_off_event(); + } else { + // We've lost radio contact + // ------------------------ + failsafe_radio_on_event(); + } + + // update AP_Notify + AP_Notify::flags.failsafe_radio = b; + } +} + + +// --------------------------------------------- +void Blimp::set_failsafe_gcs(bool b) +{ + failsafe.gcs = b; + + // update AP_Notify + AP_Notify::flags.failsafe_gcs = b; +} diff --git a/Blimp/Blimp.cpp b/Blimp/Blimp.cpp new file mode 100644 index 0000000000..a9963e91ee --- /dev/null +++ b/Blimp/Blimp.cpp @@ -0,0 +1,391 @@ +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ + +#include "Blimp.h" + +#define FORCE_VERSION_H_INCLUDE +#include "version.h" +#undef FORCE_VERSION_H_INCLUDE + +const AP_HAL::HAL& hal = AP_HAL::get_HAL(); + +#define SCHED_TASK(func, rate_hz, max_time_micros) SCHED_TASK_CLASS(Blimp, &blimp, func, rate_hz, max_time_micros) + +/* + scheduler table for fast CPUs - all regular tasks apart from the fast_loop() + should be listed here, along with how often they should be called (in hz) + and the maximum time they are expected to take (in microseconds) + */ +const AP_Scheduler::Task Blimp::scheduler_tasks[] = { + SCHED_TASK(rc_loop, 100, 130), + SCHED_TASK(throttle_loop, 50, 75), + SCHED_TASK_CLASS(AP_GPS, &blimp.gps, update, 50, 200), + SCHED_TASK(update_batt_compass, 10, 120), + SCHED_TASK_CLASS(RC_Channels, (RC_Channels*)&blimp.g2.rc_channels, read_aux_all, 10, 50), + SCHED_TASK(arm_motors_check, 10, 50), + // SCHED_TASK(auto_disarm_check, 10, 50), + // SCHED_TASK(auto_trim, 10, 75), + SCHED_TASK(update_altitude, 10, 100), + // SCHED_TASK(run_nav_updates, 50, 100), + // SCHED_TASK(update_throttle_hover,100, 90), + SCHED_TASK(three_hz_loop, 3, 75), + SCHED_TASK_CLASS(AP_ServoRelayEvents, &blimp.ServoRelayEvents, update_events, 50, 75), + SCHED_TASK_CLASS(AP_Baro, &blimp.barometer, accumulate, 50, 90), +#if LOGGING_ENABLED == ENABLED + SCHED_TASK(fourhundred_hz_logging,400, 50), +#endif + SCHED_TASK_CLASS(AP_Notify, &blimp.notify, update, 50, 90), + SCHED_TASK(one_hz_loop, 1, 100), + SCHED_TASK(ekf_check, 10, 75), + SCHED_TASK(check_vibration, 10, 50), + // SCHED_TASK(gpsglitch_check, 10, 50), + // SCHED_TASK(landinggear_update, 10, 75), + // SCHED_TASK(standby_update, 100, 75), + // SCHED_TASK(lost_vehicle_check, 10, 50), + SCHED_TASK_CLASS(GCS, (GCS*)&blimp._gcs, update_receive, 400, 180), + SCHED_TASK_CLASS(GCS, (GCS*)&blimp._gcs, update_send, 400, 550), +#if LOGGING_ENABLED == ENABLED + SCHED_TASK(ten_hz_logging_loop, 10, 350), + SCHED_TASK(twentyfive_hz_logging, 25, 110), + SCHED_TASK_CLASS(AP_Logger, &blimp.logger, periodic_tasks, 400, 300), +#endif + SCHED_TASK_CLASS(AP_InertialSensor, &blimp.ins, periodic, 400, 50), + + SCHED_TASK_CLASS(AP_Scheduler, &blimp.scheduler, update_logging, 0.1, 75), + SCHED_TASK(compass_cal_update, 100, 100), + SCHED_TASK(accel_cal_update, 10, 100), + // SCHED_TASK_CLASS(AP_TempCalibration, &blimp.g2.temp_calibration, update, 10, 100), + +#if STATS_ENABLED == ENABLED + SCHED_TASK_CLASS(AP_Stats, &blimp.g2.stats, update, 1, 100), +#endif +}; + +void Blimp::get_scheduler_tasks(const AP_Scheduler::Task *&tasks, + uint8_t &task_count, + uint32_t &log_bit) +{ + tasks = &scheduler_tasks[0]; + task_count = ARRAY_SIZE(scheduler_tasks); + log_bit = MASK_LOG_PM; +} + +constexpr int8_t Blimp::_failsafe_priorities[4]; + +// Main loop - 50hz +void Blimp::fast_loop() +{ + // update INS immediately to get current gyro data populated + ins.update(); + + // run low level rate controllers that only require IMU data + // attitude_control->rate_controller_run(); + + // send outputs to the motors library immediately + motors_output(); + + // run EKF state estimator (expensive) + // -------------------- + read_AHRS(); + + // Inertial Nav + // -------------------- + read_inertia(); + + // check if ekf has reset target heading or position + check_ekf_reset(); + + // run the attitude controllers + update_flight_mode(); + + // update home from EKF if necessary + update_home_from_EKF(); + + // check if we've landed or crashed + // Skip for now since Blimp won't land + // update_land_and_crash_detectors(); + + // log sensor health + if (should_log(MASK_LOG_ANY)) { + Log_Sensor_Health(); + } + + AP_Vehicle::fast_loop(); //just does gyro fft +} + +// get_non_takeoff_throttle - a throttle somewhere between min and mid throttle which should not lead to a takeoff +//copied in from Copter's Attitude.cpp +float Blimp::get_non_takeoff_throttle() +{ + // return MAX(0,motors->get_throttle_hover()/2.0f); + return 0.0f; //MIR no idle throttle. +} + +// start takeoff to given altitude (for use by scripting) +// bool Blimp::start_takeoff(float alt) +// { +// // exit if vehicle is not in Guided mode or Auto-Guided mode +// if (!flightmode->in_guided_mode()) { +// return false; +// } + +// if (mode_guided.do_user_takeoff_start(alt * 100.0f)) { +// blimp.set_auto_armed(true); +// return true; +// } +// return false; +// } + +// set target location (for use by scripting) +// bool Blimp::set_target_location(const Location& target_loc) +// { +// // exit if vehicle is not in Guided mode or Auto-Guided mode +// if (!flightmode->in_guided_mode()) { +// return false; +// } + +// return mode_guided.set_destination(target_loc); +// } + +// bool Blimp::set_target_velocity_NED(const Vector3f& vel_ned) +// { +// // exit if vehicle is not in Guided mode or Auto-Guided mode +// if (!flightmode->in_guided_mode()) { +// return false; +// } + +// // convert vector to neu in cm +// const Vector3f vel_neu_cms(vel_ned.x * 100.0f, vel_ned.y * 100.0f, -vel_ned.z * 100.0f); +// mode_guided.set_velocity(vel_neu_cms); +// return true; +// } + +// bool Blimp::set_target_angle_and_climbrate(float roll_deg, float pitch_deg, float yaw_deg, float climb_rate_ms, bool use_yaw_rate, float yaw_rate_degs) +// { +// // exit if vehicle is not in Guided mode or Auto-Guided mode +// if (!flightmode->in_guided_mode()) { +// return false; +// } + +// Quaternion q; +// q.from_euler(radians(roll_deg),radians(pitch_deg),radians(yaw_deg)); + +// mode_guided.set_angle(q, climb_rate_ms*100, use_yaw_rate, radians(yaw_rate_degs), false); +// return true; +// } + + +// rc_loops - reads user input from transmitter/receiver +// called at 100hz +void Blimp::rc_loop() +{ + // Read radio and 3-position switch on radio + // ----------------------------------------- + read_radio(); + rc().read_mode_switch(); +} + +// throttle_loop - should be run at 50 hz +// --------------------------- +void Blimp::throttle_loop() +{ + // update throttle_low_comp value (controls priority of throttle vs attitude control) + // update_throttle_mix(); + + // check auto_armed status + update_auto_armed(); +} + +// update_batt_compass - read battery and compass +// should be called at 10hz +void Blimp::update_batt_compass(void) +{ + // read battery before compass because it may be used for motor interference compensation + battery.read(); + + if (AP::compass().enabled()) { + // update compass with throttle value - used for compassmot + // compass.set_throttle(motors->get_throttle()); + compass.set_voltage(battery.voltage()); + compass.read(); + } +} + +// Full rate logging of attitude, rate and pid loops +// should be run at 400hz +void Blimp::fourhundred_hz_logging() +{ + if (should_log(MASK_LOG_ATTITUDE_FAST) && !blimp.flightmode->logs_attitude()) { + Log_Write_Attitude(); + } +} + +// ten_hz_logging_loop +// should be run at 10hz +void Blimp::ten_hz_logging_loop() +{ + // log attitude data if we're not already logging at the higher rate + if (should_log(MASK_LOG_ATTITUDE_MED) && !should_log(MASK_LOG_ATTITUDE_FAST) && !blimp.flightmode->logs_attitude()) { + Log_Write_Attitude(); + } + // log EKF attitude data + if (should_log(MASK_LOG_ATTITUDE_MED) || should_log(MASK_LOG_ATTITUDE_FAST)) { + Log_Write_EKF_POS(); + } + if (should_log(MASK_LOG_MOTBATT)) { + Log_Write_MotBatt(); + } + if (should_log(MASK_LOG_RCIN)) { + logger.Write_RCIN(); + if (rssi.enabled()) { + logger.Write_RSSI(); + } + } + if (should_log(MASK_LOG_RCOUT)) { + logger.Write_RCOUT(); + } + if (should_log(MASK_LOG_IMU) || should_log(MASK_LOG_IMU_FAST) || should_log(MASK_LOG_IMU_RAW)) { + logger.Write_Vibration(); + } +} + + +// twentyfive_hz_logging - should be run at 25hz +void Blimp::twentyfive_hz_logging() +{ +#if HIL_MODE != HIL_MODE_DISABLED + // HIL for a blimp needs very fast update of the servo values + gcs().send_message(MSG_SERVO_OUTPUT_RAW); +#endif + +#if HIL_MODE == HIL_MODE_DISABLED + if (should_log(MASK_LOG_ATTITUDE_FAST)) { + Log_Write_EKF_POS(); + } + + if (should_log(MASK_LOG_IMU)) { + logger.Write_IMU(); + } +#endif + +} + +// three_hz_loop - 3.3hz loop +void Blimp::three_hz_loop() +{ + // check if we've lost contact with the ground station + failsafe_gcs_check(); + + // check if we've lost terrain data + // failsafe_terrain_check(); +} + +// one_hz_loop - runs at 1Hz +void Blimp::one_hz_loop() +{ + if (should_log(MASK_LOG_ANY)) { + Log_Write_Data(LogDataID::AP_STATE, ap.value); + } + + arming.update(); + + if (!motors->armed()) { + // make it possible to change ahrs orientation at runtime during initial config + ahrs.update_orientation(); + + // update_using_interlock(); + } + + // update assigned functions and enable auxiliary servos + SRV_Channels::enable_aux_servos(); + + AP_Notify::flags.flying = !ap.land_complete; +} + +void Blimp::read_AHRS(void) +{ + // Perform IMU calculations and get attitude info + //----------------------------------------------- +#if HIL_MODE != HIL_MODE_DISABLED + // update hil before ahrs update + gcs().update(); +#endif + + // we tell AHRS to skip INS update as we have already done it in fast_loop() + ahrs.update(true); +} + +// read baro and log control tuning +void Blimp::update_altitude() +{ + // read in baro altitude + read_barometer(); + + if (should_log(MASK_LOG_CTUN)) { + Log_Write_Control_Tuning(); +#if HAL_GYROFFT_ENABLED + gyro_fft.write_log_messages(); +#else + write_notch_log_messages(); +#endif + } +} + +// vehicle specific waypoint info helpers +bool Blimp::get_wp_distance_m(float &distance) const +{ + // see GCS_MAVLINK_Blimp::send_nav_controller_output() + distance = flightmode->wp_distance() * 0.01; + return true; +} + +// vehicle specific waypoint info helpers +bool Blimp::get_wp_bearing_deg(float &bearing) const +{ + // see GCS_MAVLINK_Blimp::send_nav_controller_output() + bearing = flightmode->wp_bearing() * 0.01; + return true; +} + +// vehicle specific waypoint info helpers +bool Blimp::get_wp_crosstrack_error_m(float &xtrack_error) const +{ + // see GCS_MAVLINK_Blimp::send_nav_controller_output() + xtrack_error = flightmode->crosstrack_error() * 0.01; + return true; +} + +/* + constructor for main Blimp class + */ +Blimp::Blimp(void) + : logger(g.log_bitmask), + flight_modes(&g.flight_mode1), + control_mode(Mode::Number::MANUAL), + land_accel_ef_filter(LAND_DETECTOR_ACCEL_LPF_CUTOFF), + rc_throttle_control_in_filter(1.0f), + inertial_nav(ahrs), + param_loader(var_info), + flightmode(&mode_manual) +{ + // init sensor error logging flags + sensor_health.baro = true; + sensor_health.compass = true; +} + +Blimp blimp; +AP_Vehicle& vehicle = blimp; + +AP_HAL_MAIN_CALLBACKS(&blimp); diff --git a/Blimp/Blimp.h b/Blimp/Blimp.h new file mode 100644 index 0000000000..54a57a9ae6 --- /dev/null +++ b/Blimp/Blimp.h @@ -0,0 +1,544 @@ +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ +#pragma once +/* + This is the main Blimp class + */ + +//////////////////////////////////////////////////////////////////////////////// +// Header includes +//////////////////////////////////////////////////////////////////////////////// + +#include +#include +#include + +#include + +// Common dependencies +#include +#include +#include +#include + +// Application dependencies +#include +#include // ArduPilot Mega Flash Memory Library +#include // ArduPilot Mega Vector/Matrix math Library +// #include // interface and maths for accelerometer calibration +// #include // ArduPilot Mega Inertial Sensor (accel & gyro) Library +#include +// #include // Mission command library +// #include // Attitude control library +// #include // Attitude control library for traditional helicopter +// #include // Position control library +// #include // AP Motors library +#include // statistics library +#include // Filter library +#include // needed for AHRS build +#include // needed for AHRS build +#include // ArduPilot Mega inertial navigation library +// #include // Blimp waypoint navigation library +// #include +// #include // circle navigation library +// #include // ArduPilot Mega Declination Helper Library +#include // RC input mapping library +#include // Battery monitor library +// #include // Landing Gear library +// #include // Pilot input handling library +// #include // Heli specific pilot input handling library +#include +// #include +// #include +// #include +// #include +// #include +// #include +#include + +// Configuration +#include "defines.h" +#include "config.h" + +#include "Fins.h" + +// #define MOTOR_CLASS Fins + +#include "RC_Channel.h" // RC Channel Library + +#include "GCS_Mavlink.h" +#include "GCS_Blimp.h" +// #include "AP_Rally.h" // Rally point library +#include "AP_Arming.h" + + +#include + +// Local modules + +#include "Parameters.h" + + +#if CONFIG_HAL_BOARD == HAL_BOARD_SITL +#include +#endif + +#include "mode.h" + +class Blimp : public AP_Vehicle +{ +public: + friend class GCS_MAVLINK_Blimp; + friend class GCS_Blimp; + friend class Parameters; + friend class ParametersG2; + + friend class AP_Arming_Blimp; + friend class RC_Channel_Blimp; + friend class RC_Channels_Blimp; + + friend class Mode; + friend class ModeManual; + friend class ModeLand; + + friend class Fins; + + Blimp(void); + +private: + + // key aircraft parameters passed to multiple libraries + AP_Vehicle::MultiCopter aparm; + + // Global parameters are all contained within the 'g' class. + Parameters g; + ParametersG2 g2; + + // used to detect MAVLink acks from GCS to stop compassmot + uint8_t command_ack_counter; + + // primary input control channels + RC_Channel *channel_right; + RC_Channel *channel_front; + RC_Channel *channel_down; + RC_Channel *channel_yaw; + + AP_Logger logger; + + // flight modes convenience array + AP_Int8 *flight_modes; + const uint8_t num_flight_modes = 6; + + +#if CONFIG_HAL_BOARD == HAL_BOARD_SITL + SITL::SITL sitl; +#endif + + // Arming/Disarming management class + AP_Arming_Blimp arming; + + // system time in milliseconds of last recorded yaw reset from ekf + uint32_t ekfYawReset_ms; + int8_t ekf_primary_core; + + // vibration check + struct { + bool high_vibes; // true while high vibration are detected + uint32_t start_ms; // system time high vibration were last detected + uint32_t clear_ms; // system time high vibrations stopped + } vibration_check; + + // GCS selection + GCS_Blimp _gcs; // avoid using this; use gcs() + GCS_Blimp &gcs() + { + return _gcs; + } + + // Documentation of Globals: + typedef union { + struct { + uint8_t unused1 : 1; // 0 + uint8_t pre_arm_rc_check : 1; // 3 // true if rc input pre-arm checks have been completed successfully + uint8_t pre_arm_check : 1; // 4 // true if all pre-arm checks (rc, accel calibration, gps lock) have been performed + uint8_t auto_armed : 1; // 5 // stops auto missions from beginning until throttle is raised + uint8_t logging_started : 1; // 6 // true if logging has started + uint8_t land_complete : 1; // 7 // true if we have detected a landing + uint8_t new_radio_frame : 1; // 8 // Set true if we have new PWM data to act on from the Radio + uint8_t usb_connected_unused : 1; // 9 // UNUSED + uint8_t rc_receiver_present : 1; // 10 // true if we have an rc receiver present (i.e. if we've ever received an update + uint8_t compass_mot : 1; // 11 // true if we are currently performing compassmot calibration + uint8_t motor_test : 1; // 12 // true if we are currently performing the motors test + uint8_t initialised : 1; // 13 // true once the init_ardupilot function has completed. Extended status to GCS is not sent until this completes + uint8_t land_complete_maybe : 1; // 14 // true if we may have landed (less strict version of land_complete) + uint8_t throttle_zero : 1; // 15 // true if the throttle stick is at zero, debounced, determines if pilot intends shut-down when not using motor interlock + uint8_t system_time_set_unused : 1; // 16 // true if the system time has been set from the GPS + uint8_t gps_glitching : 1; // 17 // true if GPS glitching is affecting navigation accuracy + uint8_t using_interlock : 1; // 20 // aux switch motor interlock function is in use + uint8_t land_repo_active : 1; // 21 // true if the pilot is overriding the landing position + uint8_t motor_interlock_switch : 1; // 22 // true if pilot is requesting motor interlock enable + uint8_t in_arming_delay : 1; // 23 // true while we are armed but waiting to spin motors + uint8_t initialised_params : 1; // 24 // true when the all parameters have been initialised. we cannot send parameters to the GCS until this is done + uint8_t unused3 : 1; // 25 // was compass_init_location; true when the compass's initial location has been set + uint8_t unused2 : 1; // 26 // aux switch rc_override is allowed + uint8_t armed_with_switch : 1; // 27 // we armed using a arming switch + }; + uint32_t value; + } ap_t; + + ap_t ap; //MIR Set of general variables + + static_assert(sizeof(uint32_t) == sizeof(ap), "ap_t must be uint32_t"); + + // This is the state of the flight control system + // There are multiple states defined such as STABILIZE, ACRO, + Mode::Number control_mode; + ModeReason control_mode_reason = ModeReason::UNKNOWN; + Mode::Number prev_control_mode; + + RCMapper rcmap; + + // intertial nav alt when we armed + float arming_altitude_m; + + // Failsafe + struct { + uint32_t last_heartbeat_ms; // the time when the last HEARTBEAT message arrived from a GCS - used for triggering gcs failsafe + + int8_t radio_counter; // number of iterations with throttle below throttle_fs_value + + uint8_t radio : 1; // A status flag for the radio failsafe + uint8_t gcs : 1; // A status flag for the ground station failsafe + uint8_t ekf : 1; // true if ekf failsafe has occurred + } failsafe; + + bool any_failsafe_triggered() const + { + return failsafe.radio || battery.has_failsafed() || failsafe.gcs || failsafe.ekf; + } + + // sensor health for logging + struct { + uint8_t baro : 1; // true if baro is healthy + uint8_t compass : 1; // true if compass is healthy + uint8_t primary_gps : 2; // primary gps index + } sensor_health; + + // Motor Output + Fins *motors; + + int32_t _home_bearing; + uint32_t _home_distance; + + // Stores initial bearing when armed - initial simple bearing is modified in super simple mode so not suitable + int32_t initial_armed_bearing; + + // Battery Sensors + AP_BattMonitor battery{MASK_LOG_CURRENT, + FUNCTOR_BIND_MEMBER(&Blimp::handle_battery_failsafe, void, const char*, const int8_t), + _failsafe_priorities}; + + // Altitude + int32_t baro_alt; // barometer altitude in cm above home + LowPassFilterVector3f land_accel_ef_filter; // accelerations for land and crash detector tests + + // filtered pilot's throttle input used to cancel landing if throttle held high + LowPassFilterFloat rc_throttle_control_in_filter; + + // 3D Location vectors + // Current location of the vehicle (altitude is relative to home) + Location current_loc; + + // Inertial Navigation + AP_InertialNav_NavEKF inertial_nav; + + // Attitude, Position and Waypoint navigation objects + // To-Do: move inertial nav up or other navigation variables down here + // AC_AttitudeControl_t *attitude_control; + // AC_PosControl *pos_control; + // AC_WPNav *wp_nav; + // AC_Loiter *loiter_nav; + + + // System Timers + // -------------- + // arm_time_ms - Records when vehicle was armed. Will be Zero if we are disarmed. + uint32_t arm_time_ms; + + // Used to exit the roll and pitch auto trim function + uint8_t auto_trim_counter; + bool auto_trim_started = false; + + + // last valid RC input time + uint32_t last_radio_update_ms; + + // Top-level logic + // setup the var_info table + AP_Param param_loader; + + bool standby_active; + + static const AP_Scheduler::Task scheduler_tasks[]; + static const AP_Param::Info var_info[]; + static const struct LogStructure log_structure[]; + + enum Failsafe_Action { + Failsafe_Action_None = 0, + Failsafe_Action_Land = 1, + Failsafe_Action_Terminate = 5 + }; + + enum class FailsafeOption { + RC_CONTINUE_IF_AUTO = (1<<0), // 1 + GCS_CONTINUE_IF_AUTO = (1<<1), // 2 + RC_CONTINUE_IF_GUIDED = (1<<2), // 4 + CONTINUE_IF_LANDING = (1<<3), // 8 + GCS_CONTINUE_IF_PILOT_CONTROL = (1<<4), // 16 + RELEASE_GRIPPER = (1<<5), // 32 + }; + + static constexpr int8_t _failsafe_priorities[] = { + Failsafe_Action_Terminate, + Failsafe_Action_Land, + Failsafe_Action_None, + -1 // the priority list must end with a sentinel of -1 + }; + +#define FAILSAFE_LAND_PRIORITY 1 + static_assert(_failsafe_priorities[FAILSAFE_LAND_PRIORITY] == Failsafe_Action_Land, + "FAILSAFE_LAND_PRIORITY must match the entry in _failsafe_priorities"); + static_assert(_failsafe_priorities[ARRAY_SIZE(_failsafe_priorities) - 1] == -1, + "_failsafe_priorities is missing the sentinel"); + + // AP_State.cpp + void set_auto_armed(bool b); + void set_failsafe_radio(bool b); + void set_failsafe_gcs(bool b); + // void update_using_interlock(); + + // Blimp.cpp + void get_scheduler_tasks(const AP_Scheduler::Task *&tasks, + uint8_t &task_count, + uint32_t &log_bit) override; + void fast_loop() override; + // bool start_takeoff(float alt) override; + // bool set_target_location(const Location& target_loc) override; + // bool set_target_velocity_NED(const Vector3f& vel_ned) override; + // bool set_target_angle_and_climbrate(float roll_deg, float pitch_deg, float yaw_deg, float climb_rate_ms, bool use_yaw_rate, float yaw_rate_degs) override; + void rc_loop(); + void throttle_loop(); + void update_batt_compass(void); + void fourhundred_hz_logging(); + void ten_hz_logging_loop(); + void twentyfive_hz_logging(); + void three_hz_loop(); + void one_hz_loop(); + void read_AHRS(void); + void update_altitude(); + + // Attitude.cpp + float get_pilot_desired_yaw_rate(int16_t stick_angle); + // void update_throttle_hover(); + float get_pilot_desired_climb_rate(float throttle_control); + float get_non_takeoff_throttle(); + // void set_accel_throttle_I_from_pilot_throttle(); + void rotate_body_frame_to_NE(float &x, float &y); + uint16_t get_pilot_speed_dn(); + + // baro_ground_effect.cpp + // void update_ground_effect_detector(void); + // void update_ekf_terrain_height_stable(); + + // commands.cpp + void update_home_from_EKF(); + void set_home_to_current_location_inflight(); + bool set_home_to_current_location(bool lock) WARN_IF_UNUSED; + bool set_home(const Location& loc, bool lock) WARN_IF_UNUSED; + bool far_from_EKF_origin(const Location& loc); + + // compassmot.cpp + // MAV_RESULT mavlink_compassmot(const GCS_MAVLINK &gcs_chan); + + // // crash_check.cpp + // void crash_check(); + // void thrust_loss_check(); + // void parachute_check(); + // void parachute_release(); + // void parachute_manual_release(); + + // ekf_check.cpp + void ekf_check(); + bool ekf_over_threshold(); + void failsafe_ekf_event(); + void failsafe_ekf_off_event(void); + void check_ekf_reset(); + void check_vibration(); + + // events.cpp + bool failsafe_option(FailsafeOption opt) const; + void failsafe_radio_on_event(); + void failsafe_radio_off_event(); + void handle_battery_failsafe(const char* type_str, const int8_t action); + void failsafe_gcs_check(); + // void failsafe_gcs_on_event(void); //MIR will probably need these two soon. + // void failsafe_gcs_off_event(void); + // void gpsglitch_check(); + // void set_mode_RTL_or_land_with_pause(ModeReason reason); + bool should_disarm_on_failsafe(); + void do_failsafe_action(Failsafe_Action action, ModeReason reason); + + // failsafe.cpp + void failsafe_enable(); + void failsafe_disable(); + + // fence.cpp + void fence_check(); + + // inertia.cpp + void read_inertia(); + + // landing_detector.cpp + void update_land_and_crash_detectors(); + void update_land_detector(); + void set_land_complete(bool b); + void set_land_complete_maybe(bool b); + // void update_throttle_mix(); + + // landing_gear.cpp + void landinggear_update(); + + // // standby.cpp + // void standby_update(); + + // Log.cpp + void Log_Write_Control_Tuning(); + void Log_Write_Performance(); + void Log_Write_Attitude(); + void Log_Write_EKF_POS(); + void Log_Write_MotBatt(); + void Log_Write_Data(LogDataID id, int32_t value); + void Log_Write_Data(LogDataID id, uint32_t value); + void Log_Write_Data(LogDataID id, int16_t value); + void Log_Write_Data(LogDataID id, uint16_t value); + void Log_Write_Data(LogDataID id, float value); + void Log_Write_Parameter_Tuning(uint8_t param, float tuning_val, float tune_min, float tune_max); + void Log_Sensor_Health(); + void Log_Write_Precland(); + void Log_Write_GuidedTarget(uint8_t target_type, const Vector3f& pos_target, const Vector3f& vel_target); + void Log_Write_SysID_Setup(uint8_t systemID_axis, float waveform_magnitude, float frequency_start, float frequency_stop, float time_fade_in, float time_const_freq, float time_record, float time_fade_out); + void Log_Write_SysID_Data(float waveform_time, float waveform_sample, float waveform_freq, float angle_x, float angle_y, float angle_z, float accel_x, float accel_y, float accel_z); + void Log_Write_Vehicle_Startup_Messages(); + void log_init(void); + + // mode.cpp + bool set_mode(Mode::Number mode, ModeReason reason); + bool set_mode(const uint8_t new_mode, const ModeReason reason) override; + uint8_t get_mode() const override + { + return (uint8_t)control_mode; + } + void update_flight_mode(); + void notify_flight_mode(); + + // mode_land.cpp + void set_mode_land_with_pause(ModeReason reason); + bool landing_with_GPS(); + + // // motors.cpp + void arm_motors_check(); + // void auto_disarm_check(); + void motors_output(); + // void lost_vehicle_check(); + + // navigation.cpp + // void run_nav_updates(void); + // int32_t home_bearing(); + // uint32_t home_distance(); + + // Parameters.cpp + void load_parameters(void) override; + void convert_pid_parameters(void); + void convert_lgr_parameters(void); + void convert_fs_options_params(void); + + // radio.cpp + void default_dead_zones(); + void init_rc_in(); + void init_rc_out(); + void enable_motor_output(); + void read_radio(); + void set_throttle_and_failsafe(uint16_t throttle_pwm); + void set_throttle_zero_flag(int16_t throttle_control); + // void radio_passthrough_to_motors(); + int16_t get_throttle_mid(void); + + // sensors.cpp + void read_barometer(void); + void init_rangefinder(void); + void read_rangefinder(void); + bool rangefinder_alt_ok(); + bool rangefinder_up_ok(); + void rpm_update(); + void init_optflow(); + void update_optical_flow(void); + void compass_cal_update(void); + void accel_cal_update(void); + void init_proximity(); + void update_proximity(); + + // RC_Channel.cpp + void save_trim(); + void auto_trim(); + void auto_trim_cancel(); + + // system.cpp + void init_ardupilot() override; + void startup_INS_ground(); + // void update_dynamic_notch() override; + bool position_ok() const; + bool ekf_has_absolute_position() const; + bool ekf_has_relative_position() const; + bool ekf_alt_ok() const; + void update_auto_armed(); + bool should_log(uint32_t mask); + MAV_TYPE get_frame_mav_type(); + const char* get_frame_string(); + void allocate_motors(void); + + // // tuning.cpp + // void tuning(); + + // vehicle specific waypoint info helpers + bool get_wp_distance_m(float &distance) const override; + bool get_wp_bearing_deg(float &bearing) const override; + bool get_wp_crosstrack_error_m(float &xtrack_error) const override; + + Mode *flightmode; + ModeManual mode_manual; + ModeLand mode_land; + + // mode.cpp + Mode *mode_from_mode_num(const Mode::Number mode); + void exit_mode(Mode *&old_flightmode, Mode *&new_flightmode); + +public: + void failsafe_check(); // failsafe.cpp +}; + +extern Blimp blimp; + +using AP_HAL::millis; +using AP_HAL::micros; diff --git a/Blimp/Fins.cpp b/Blimp/Fins.cpp new file mode 100644 index 0000000000..c665a40ab3 --- /dev/null +++ b/Blimp/Fins.cpp @@ -0,0 +1,153 @@ +#include "Blimp.h" + +//most of these will become parameters... +//put here instead of Fins.h so that it can be changed without having to recompile the entire vehicle + +#define FIN_SCALE_MAX 1000 + +/* + 2nd group of parameters + */ +const AP_Param::GroupInfo Fins::var_info[] = { + + // @Param: FREQ_HZ + // @DisplayName: Fins frequency + // @Description: This is the oscillation frequency of the fins + // @Range: 1 10 + // @User: Standard + AP_GROUPINFO("FREQ_HZ", 1, Fins, freq_hz, 3), + + // @Param: TURBO_MODE + // @DisplayName: Enable turbo mode + // @Description: Enables double speed on high offset. + // @Range: 0 1 + // @User: Standard + AP_GROUPINFO("TURBO_MODE", 2, Fins, turbo_mode, 0), + + AP_GROUPEND +}; + +//constructor +Fins::Fins(uint16_t loop_rate) : + _loop_rate(loop_rate) +{ + AP_Param::setup_object_defaults(this, var_info); +} + +void Fins::setup_fins() +{ + //amp r f d y,off r f d y right, front, down, yaw + add_fin(0, 0, -1, 0.5, 0, 0, 0, -0.5, 0); //Back(?) + add_fin(1, 0, 1, 0.5, 0, 0, 0, -0.5, 0); //Front(?) + add_fin(2, -1, 0, 0, 0.5, 0, 0, 0, 0.5); //Right + add_fin(3, 1, 0, 0, 0.5, 0, 0, 0, -0.5); //Left + + SRV_Channels::set_angle(SRV_Channel::k_motor1, FIN_SCALE_MAX); + SRV_Channels::set_angle(SRV_Channel::k_motor2, FIN_SCALE_MAX); + SRV_Channels::set_angle(SRV_Channel::k_motor3, FIN_SCALE_MAX); + SRV_Channels::set_angle(SRV_Channel::k_motor4, FIN_SCALE_MAX); + + GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "MIR: All fins have been added."); +} + +void Fins::add_fin(int8_t fin_num, float right_amp_fac, float front_amp_fac, float down_amp_fac, float yaw_amp_fac, + float right_off_fac, float front_off_fac, float down_off_fac, float yaw_off_fac) +{ + + // ensure valid fin number is provided + if (fin_num >= 0 && fin_num < NUM_FINS) { + + // set amplitude factors + _right_amp_factor[fin_num] = right_amp_fac; + _front_amp_factor[fin_num] = front_amp_fac; + _down_amp_factor[fin_num] = down_amp_fac; + _yaw_amp_factor[fin_num] = yaw_amp_fac; + + // set offset factors + _right_off_factor[fin_num] = right_off_fac; + _front_off_factor[fin_num] = front_off_fac; + _down_off_factor[fin_num] = down_off_fac; + _yaw_off_factor[fin_num] = yaw_off_fac; + } +} + +//B,F,R,L = 0,1,2,3 +void Fins::output() +{ + if (!_armed) { + // set everything to zero so fins stop moving + right_out = 0; + front_out = 0; + down_out = 0; + yaw_out = 0; + } + + right_out /= RC_SCALE; + front_out /= RC_SCALE; + down_out /= RC_SCALE; + yaw_out /= RC_SCALE; + + _time = AP_HAL::micros() * 1.0e-6; + + for (int8_t i=0; i 0.0f) { + _num_added++; + } + if (max(0,_front_amp_factor[i]*front_out) > 0.0f) { + _num_added++; + } + if (fabsf(_down_amp_factor[i]*down_out) > 0.0f) { + _num_added++; + } + if (fabsf(_yaw_amp_factor[i]*yaw_out) > 0.0f) { + _num_added++; + } + + if (_num_added > 0) { + _off[i] = _off[i]/_num_added; //average the offsets + } + + if ((_amp[i]+fabsf(_off[i])) > 1) { + _amp[i] = 1 - fabsf(_off[i]); + } + + if (turbo_mode) { + //double speed fins if offset at max... MIR + if (_amp[i] <= 0.6 && fabsf(_off[i]) >= 0.4) { + _omm[i] = 2; + } + } + + // finding and outputting current position for each servo from sine wave + _pos[i]= _amp[i]*cosf(freq_hz * _omm[i] * _time * 2 * M_PI) + _off[i]; //removed +MAX_AMP because output can do -ve numbers + SRV_Channels::set_output_scaled(SRV_Channels::get_motor_function(i), _pos[i] * FIN_SCALE_MAX); + } + + ::printf("FINS (amp %.1f %.1f %.1f %.1f off %.1f %.1f %.1f %.1f omm %.1f %.1f %.1f %.1f)\n", + _amp[0], _amp[1], _amp[2], _amp[3], _off[0], _off[1], _off[2], _off[3], _omm[0], _omm[1], _omm[2], _omm[3]); +} + +void Fins::output_min() +{ + right_out = 0; + front_out = 0; + down_out = 0; + yaw_out = 0; + Fins::output(); +} + +// MIR - Probably want to completely get rid of the desired spool state thing. +void Fins::set_desired_spool_state(DesiredSpoolState spool) +{ + if (_armed || (spool == DesiredSpoolState::SHUT_DOWN)) { + // Set DesiredSpoolState only if it is either armed or it wants to shut down. + _spool_desired = spool; + } +}; diff --git a/Blimp/Fins.h b/Blimp/Fins.h new file mode 100644 index 0000000000..2994c8b9e5 --- /dev/null +++ b/Blimp/Fins.h @@ -0,0 +1,175 @@ +//This class converts horizontal acceleration commands to fin flapping commands. +#pragma once +// #include +// #include // ArduPilot Mega Vector/Matrix math Library +#include // Notify library +#include +// #include // filter library +// #include +// #include + +extern const AP_HAL::HAL& hal; + +// #define FINS_SPEED_DEFAULT 10 //MIR what is this? +#define NUM_FINS 4 +#define RC_SCALE 1000 +class Fins +{ +public: + friend class Blimp; + // Fins(void); + + enum motor_frame_class { + MOTOR_FRAME_UNDEFINED = 0, + MOTOR_FRAME_AIRFISH = 1, + }; + enum motor_frame_type { + MOTOR_FRAME_TYPE_AIRFISH = 1, + }; + + //constructor + Fins(uint16_t loop_rate); + + // var_info for holding Parameter information + static const struct AP_Param::GroupInfo var_info[]; + + // singleton support + // static Fins *get_singleton(void) { return _singleton; } + + // desired spool states + // from AP_Motors_Class.h + enum class DesiredSpoolState : uint8_t { + SHUT_DOWN = 0, // all motors should move to stop + // GROUND_IDLE = 1, // all motors should move to ground idle + THROTTLE_UNLIMITED = 2, // motors should move to being a state where throttle is unconstrained (e.g. by start up procedure) + }; + + // spool states + enum class SpoolState : uint8_t { + SHUT_DOWN = 0, // all motors stop + // GROUND_IDLE = 1, // all motors at ground idle + // SPOOLING_UP = 2, // increasing maximum throttle while stabilizing + THROTTLE_UNLIMITED = 3, // throttle is no longer constrained by start up procedure + // SPOOLING_DOWN = 4, // decreasing maximum throttle while stabilizing + }; + + bool initialised_ok() const + { + return true; + } + + void armed(bool arm) + { + if (arm != _armed) { + _armed = arm; + AP_Notify::flags.armed = arm; + } + + } + bool armed() const + { + return _armed; + } + +protected: + // internal variables + const uint16_t _loop_rate; // rate in Hz at which output() function is called (normally 400hz) + uint16_t _speed_hz; // speed in hz to send updates to motors + // float _roll_in; // desired roll control from attitude controllers, -1 ~ +1 + // float _roll_in_ff; // desired roll feed forward control from attitude controllers, -1 ~ +1 + // float _pitch_in; // desired pitch control from attitude controller, -1 ~ +1 + // float _pitch_in_ff; // desired pitch feed forward control from attitude controller, -1 ~ +1 + // float _yaw_in; // desired yaw control from attitude controller, -1 ~ +1 + // float _yaw_in_ff; // desired yaw feed forward control from attitude controller, -1 ~ +1 + float _throttle_in; // last throttle input from set_throttle caller + float _down_out; // throttle after mixing is complete + // float _forward_in; // last forward input from set_forward caller + // float _lateral_in; // last lateral input from set_lateral caller + float _throttle_avg_max; // last throttle input from set_throttle_avg_max + // LowPassFilterFloat _throttle_filter; // throttle input filter + DesiredSpoolState _spool_desired; // desired spool state + SpoolState _spool_state; // current spool mode + + float _air_density_ratio; //air density as a proportion of sea level density + + float _time; //current timestep + + bool _armed; // 0 if disarmed, 1 if armed + + float _amp[NUM_FINS]; //amplitudes + float _off[NUM_FINS]; //offsets + float _omm[NUM_FINS]; //omega multiplier + float _pos[NUM_FINS]; //servo positions + + float _right_amp_factor[NUM_FINS]; + float _front_amp_factor[NUM_FINS]; + float _down_amp_factor[NUM_FINS]; + float _yaw_amp_factor[NUM_FINS]; + + float _right_off_factor[NUM_FINS]; + float _front_off_factor[NUM_FINS]; + float _down_off_factor[NUM_FINS]; + float _yaw_off_factor[NUM_FINS]; + + int8_t _num_added; + // private: +public: + float right_out; //input right movement, negative for left, +1 to -1 + float front_out; //input front/forwards movement, negative for backwards, +1 to -1 + float yaw_out; //input yaw, +1 to -1 + float down_out; //input height control, +1 to -1 + + AP_Float freq_hz; + AP_Int8 turbo_mode; + + bool _interlock; // 1 if the motor interlock is enabled (i.e. motors run), 0 if disabled (motors don't run) + bool _initialised_ok; // 1 if initialisation was successful + + // get_spool_state - get current spool state + enum SpoolState get_spool_state(void) const + { + return _spool_state; + } + + // float mapf(float x, float in_min, float in_max, float out_min, float out_max) { + // return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min; } + + float max(float one, float two) + { + if (one >= two) { + return one; + } else { + return two; + } + } + + void output_min(); + + void add_fin(int8_t fin_num, float right_amp_fac, float front_amp_fac, float yaw_amp_fac, float down_amp_fac, + float right_off_fac, float front_off_fac, float yaw_off_fac, float down_off_fac); + + void setup_fins(); + + float get_throttle_hover() + { + return 0; //MIR temp + } + + void set_desired_spool_state(DesiredSpoolState spool); + + void output(); + + float get_throttle() + { + return 0.1f; //MIR temp + } + + void rc_write(uint8_t chan, uint16_t pwm); + + // set_density_ratio - sets air density as a proportion of sea level density + void set_air_density_ratio(float ratio) + { + _air_density_ratio = ratio; + } + +}; diff --git a/Blimp/GCS_Blimp.cpp b/Blimp/GCS_Blimp.cpp new file mode 100644 index 0000000000..c273c3f0ba --- /dev/null +++ b/Blimp/GCS_Blimp.cpp @@ -0,0 +1,69 @@ +#include "GCS_Blimp.h" + +#include "Blimp.h" + +uint8_t GCS_Blimp::sysid_this_mav() const +{ + return blimp.g.sysid_this_mav; +} + +const char* GCS_Blimp::frame_string() const +{ + return blimp.get_frame_string(); +} + +void GCS_Blimp::update_vehicle_sensor_status_flags(void) +{ + control_sensors_present |= + MAV_SYS_STATUS_SENSOR_ANGULAR_RATE_CONTROL | + MAV_SYS_STATUS_SENSOR_ATTITUDE_STABILIZATION | + MAV_SYS_STATUS_SENSOR_YAW_POSITION; + + control_sensors_enabled |= + MAV_SYS_STATUS_SENSOR_ANGULAR_RATE_CONTROL | + MAV_SYS_STATUS_SENSOR_ATTITUDE_STABILIZATION | + MAV_SYS_STATUS_SENSOR_YAW_POSITION; + + control_sensors_health |= + MAV_SYS_STATUS_SENSOR_ANGULAR_RATE_CONTROL | + MAV_SYS_STATUS_SENSOR_ATTITUDE_STABILIZATION | + MAV_SYS_STATUS_SENSOR_YAW_POSITION; + + const Blimp::ap_t &ap = blimp.ap; + + if (ap.rc_receiver_present) { + control_sensors_present |= MAV_SYS_STATUS_SENSOR_RC_RECEIVER; + control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_RC_RECEIVER; + } + + control_sensors_present |= MAV_SYS_STATUS_SENSOR_Z_ALTITUDE_CONTROL; + control_sensors_present |= MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL; + + // switch (blimp.control_mode) { + // case Mode::Number::AUTO: + // case Mode::Number::AVOID_ADSB: + // case Mode::Number::GUIDED: + // case Mode::Number::LOITER: + // control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_Z_ALTITUDE_CONTROL; + // control_sensors_health |= MAV_SYS_STATUS_SENSOR_Z_ALTITUDE_CONTROL; + // control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL; + // control_sensors_health |= MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL; + // break; + // case Mode::Number::ALT_HOLD: + // case Mode::Number::GUIDED_NOGPS: + // case Mode::Number::SPORT: + // case Mode::Number::AUTOTUNE: + // case Mode::Number::FLOWHOLD: + // control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_Z_ALTITUDE_CONTROL; + // control_sensors_health |= MAV_SYS_STATUS_SENSOR_Z_ALTITUDE_CONTROL; + // break; + // default: + // // stabilize, acro, drift, and flip have no automatic x,y or z control (i.e. all manual) + // break; + // } + + if (ap.rc_receiver_present && !blimp.failsafe.radio) { + control_sensors_health |= MAV_SYS_STATUS_SENSOR_RC_RECEIVER; + } + +} diff --git a/Blimp/GCS_Blimp.h b/Blimp/GCS_Blimp.h new file mode 100644 index 0000000000..1fd625a840 --- /dev/null +++ b/Blimp/GCS_Blimp.h @@ -0,0 +1,58 @@ +#pragma once + +#include +#include "GCS_Mavlink.h" + +class GCS_Blimp : public GCS +{ + friend class Blimp; // for access to _chan in parameter declarations + +public: + + // return GCS link at offset ofs + GCS_MAVLINK_Blimp *chan(const uint8_t ofs) override + { + if (ofs > _num_gcs) { + INTERNAL_ERROR(AP_InternalError::error_t::gcs_offset); + return nullptr; + } + return (GCS_MAVLINK_Blimp *)_chan[ofs]; + } + const GCS_MAVLINK_Blimp *chan(const uint8_t ofs) const override + { + if (ofs > _num_gcs) { + INTERNAL_ERROR(AP_InternalError::error_t::gcs_offset); + return nullptr; + } + return (GCS_MAVLINK_Blimp *)_chan[ofs]; + } + + void update_vehicle_sensor_status_flags(void) override; + + uint32_t custom_mode() const override; + MAV_TYPE frame_type() const override; + + const char* frame_string() const override; + + bool vehicle_initialised() const override; + +protected: + + uint8_t sysid_this_mav() const override; + + // minimum amount of time (in microseconds) that must remain in + // the main scheduler loop before we are allowed to send any + // mavlink messages. We want to prioritise the main flight + // control loop over communications + uint16_t min_loop_time_remaining_for_message_send_us() const override + { + return 250; + } + + GCS_MAVLINK_Blimp *new_gcs_mavlink_backend(GCS_MAVLINK_Parameters ¶ms, + AP_HAL::UARTDriver &uart) override + { + return new GCS_MAVLINK_Blimp(params, uart); + } + +}; diff --git a/Blimp/GCS_Mavlink.cpp b/Blimp/GCS_Mavlink.cpp new file mode 100644 index 0000000000..a467c505ab --- /dev/null +++ b/Blimp/GCS_Mavlink.cpp @@ -0,0 +1,977 @@ +#include "Blimp.h" + +#include "GCS_Mavlink.h" + +/* + * !!NOTE!! + * + * the use of NOINLINE separate functions for each message type avoids + * a compiler bug in gcc that would cause it to use far more stack + * space than is needed. Without the NOINLINE we use the sum of the + * stack needed for each message type. Please be careful to follow the + * pattern below when adding any new messages + */ + +MAV_TYPE GCS_Blimp::frame_type() const +{ + return blimp.get_frame_mav_type(); +} + +MAV_MODE GCS_MAVLINK_Blimp::base_mode() const +{ + uint8_t _base_mode = MAV_MODE_FLAG_STABILIZE_ENABLED; + // work out the base_mode. This value is not very useful + // for APM, but we calculate it as best we can so a generic + // MAVLink enabled ground station can work out something about + // what the MAV is up to. The actual bit values are highly + // ambiguous for most of the APM flight modes. In practice, you + // only get useful information from the custom_mode, which maps to + // the APM flight mode and has a well defined meaning in the + // ArduPlane documentation + + // switch (blimp.control_mode) { + // case Mode::Number::AUTO: + // case Mode::Number::RTL: + // case Mode::Number::LOITER: + // case Mode::Number::AVOID_ADSB: + // case Mode::Number::FOLLOW: + // case Mode::Number::GUIDED: + // case Mode::Number::CIRCLE: + // case Mode::Number::POSHOLD: + // case Mode::Number::BRAKE: + // case Mode::Number::SMART_RTL: + // _base_mode |= MAV_MODE_FLAG_GUIDED_ENABLED; + // // note that MAV_MODE_FLAG_AUTO_ENABLED does not match what + // // APM does in any mode, as that is defined as "system finds its own goal + // // positions", which APM does not currently do + // break; + // default: + // break; + // } + + // all modes except INITIALISING have some form of manual + // override if stick mixing is enabled + _base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED; + +#if HIL_MODE != HIL_MODE_DISABLED + _base_mode |= MAV_MODE_FLAG_HIL_ENABLED; +#endif + + // we are armed if we are not initialising + if (blimp.motors != nullptr && blimp.motors->armed()) { + _base_mode |= MAV_MODE_FLAG_SAFETY_ARMED; + } + + // indicate we have set a custom mode + _base_mode |= MAV_MODE_FLAG_CUSTOM_MODE_ENABLED; + + return (MAV_MODE)_base_mode; +} + +uint32_t GCS_Blimp::custom_mode() const +{ + return (uint32_t)blimp.control_mode; +} + +MAV_STATE GCS_MAVLINK_Blimp::vehicle_system_status() const +{ + // set system as critical if any failsafe have triggered + if (blimp.any_failsafe_triggered()) { + return MAV_STATE_CRITICAL; + } + + if (blimp.ap.land_complete) { + return MAV_STATE_STANDBY; + } + + return MAV_STATE_ACTIVE; +} + + +void GCS_MAVLINK_Blimp::send_position_target_global_int() +{ + Location target; + if (!blimp.flightmode->get_wp(target)) { + return; + } + mavlink_msg_position_target_global_int_send( + chan, + AP_HAL::millis(), // time_boot_ms + MAV_FRAME_GLOBAL, // targets are always global altitude + 0xFFF8, // ignore everything except the x/y/z components + target.lat, // latitude as 1e7 + target.lng, // longitude as 1e7 + target.alt * 0.01f, // altitude is sent as a float + 0.0f, // vx + 0.0f, // vy + 0.0f, // vz + 0.0f, // afx + 0.0f, // afy + 0.0f, // afz + 0.0f, // yaw + 0.0f); // yaw_rate +} + +// void GCS_MAVLINK_Blimp::send_position_target_local_ned() +// { +// #if MODE_GUIDED_ENABLED == ENABLED +// if (!blimp.flightmode->in_guided_mode()) { +// return; +// } + +// const GuidedMode guided_mode = blimp.mode_guided.mode(); +// Vector3f target_pos; +// Vector3f target_vel; +// uint16_t type_mask; + +// if (guided_mode == Guided_WP) { +// type_mask = 0x0FF8; // ignore everything except position +// target_pos = blimp.wp_nav->get_wp_destination() * 0.01f; // convert to metres +// } else if (guided_mode == Guided_Velocity) { +// type_mask = 0x0FC7; // ignore everything except velocity +// target_vel = blimp.flightmode->get_desired_velocity() * 0.01f; // convert to m/s +// } else { +// type_mask = 0x0FC0; // ignore everything except position & velocity +// target_pos = blimp.wp_nav->get_wp_destination() * 0.01f; +// target_vel = blimp.flightmode->get_desired_velocity() * 0.01f; +// } + +// mavlink_msg_position_target_local_ned_send( +// chan, +// AP_HAL::millis(), // time boot ms +// MAV_FRAME_LOCAL_NED, +// type_mask, +// target_pos.x, // x in metres +// target_pos.y, // y in metres +// -target_pos.z, // z in metres NED frame +// target_vel.x, // vx in m/s +// target_vel.y, // vy in m/s +// -target_vel.z, // vz in m/s NED frame +// 0.0f, // afx +// 0.0f, // afy +// 0.0f, // afz +// 0.0f, // yaw +// 0.0f); // yaw_rate +// #endif +// } + +void GCS_MAVLINK_Blimp::send_nav_controller_output() const +{ + // if (!blimp.ap.initialised) { + // return; + // } + // const Vector3f &targets = blimp.attitude_control->get_att_target_euler_cd(); + // const Mode *flightmode = blimp.flightmode; + // mavlink_msg_nav_controller_output_send( + // chan, + // targets.x * 1.0e-2f, + // targets.y * 1.0e-2f, + // targets.z * 1.0e-2f, + // flightmode->wp_bearing() * 1.0e-2f, + // MIN(flightmode->wp_distance() * 1.0e-2f, UINT16_MAX), + // blimp.pos_control->get_alt_error() * 1.0e-2f, + // 0, + // flightmode->crosstrack_error() * 1.0e-2f); +} + +float GCS_MAVLINK_Blimp::vfr_hud_airspeed() const +{ + Vector3f airspeed_vec_bf; + if (AP::ahrs().airspeed_vector_true(airspeed_vec_bf)) { + // we are running the EKF3 wind estimation code which can give + // us an airspeed estimate + return airspeed_vec_bf.length(); + } + return AP::gps().ground_speed(); +} + +int16_t GCS_MAVLINK_Blimp::vfr_hud_throttle() const +{ + if (blimp.motors == nullptr) { + return 0; + } + return (int16_t)(blimp.motors->get_throttle() * 100); +} + +/* + send PID tuning message + */ +void GCS_MAVLINK_Blimp::send_pid_tuning() +{ + static const PID_TUNING_AXIS axes[] = { + PID_TUNING_ROLL, + PID_TUNING_PITCH, + PID_TUNING_YAW, + PID_TUNING_ACCZ + }; + for (uint8_t i=0; iget_rate_roll_pid().get_pid_info(); + // break; + // case PID_TUNING_PITCH: + // pid_info = &blimp.attitude_control->get_rate_pitch_pid().get_pid_info(); + // break; + // case PID_TUNING_YAW: + // pid_info = &blimp.attitude_control->get_rate_yaw_pid().get_pid_info(); + // break; + // case PID_TUNING_ACCZ: + // pid_info = &blimp.pos_control->get_accel_z_pid().get_pid_info(); + // break; + default: + continue; + } + if (pid_info != nullptr) { + mavlink_msg_pid_tuning_send(chan, + axes[i], + pid_info->target, + pid_info->actual, + pid_info->FF, + pid_info->P, + pid_info->I, + pid_info->D); + } + } +} + +uint8_t GCS_MAVLINK_Blimp::sysid_my_gcs() const +{ + return blimp.g.sysid_my_gcs; +} +bool GCS_MAVLINK_Blimp::sysid_enforce() const +{ + return blimp.g2.sysid_enforce; +} + +uint32_t GCS_MAVLINK_Blimp::telem_delay() const +{ + return (uint32_t)(blimp.g.telem_delay); +} + +bool GCS_Blimp::vehicle_initialised() const +{ + return blimp.ap.initialised; +} + +// try to send a message, return false if it wasn't sent +bool GCS_MAVLINK_Blimp::try_send_message(enum ap_message id) +{ + switch (id) { + + case MSG_WIND: + CHECK_PAYLOAD_SIZE(WIND); + send_wind(); + break; + + case MSG_SERVO_OUT: + case MSG_AOA_SSA: + case MSG_LANDING: + case MSG_ADSB_VEHICLE: + // unused + break; + + default: + return GCS_MAVLINK::try_send_message(id); + } + return true; +} + + +const AP_Param::GroupInfo GCS_MAVLINK_Parameters::var_info[] = { + // @Param: RAW_SENS + // @DisplayName: Raw sensor stream rate + // @Description: Stream rate of RAW_IMU, SCALED_IMU2, SCALED_IMU3, SCALED_PRESSURE, SCALED_PRESSURE2, SCALED_PRESSURE3 and SENSOR_OFFSETS to ground station + // @Units: Hz + // @Range: 0 10 + // @Increment: 1 + // @User: Advanced + AP_GROUPINFO("RAW_SENS", 0, GCS_MAVLINK_Parameters, streamRates[0], 0), + + // @Param: EXT_STAT + // @DisplayName: Extended status stream rate to ground station + // @Description: Stream rate of SYS_STATUS, POWER_STATUS, MEMINFO, CURRENT_WAYPOINT, GPS_RAW_INT, GPS_RTK (if available), GPS2_RAW (if available), GPS2_RTK (if available), NAV_CONTROLLER_OUTPUT, and FENCE_STATUS to ground station + // @Units: Hz + // @Range: 0 10 + // @Increment: 1 + // @User: Advanced + AP_GROUPINFO("EXT_STAT", 1, GCS_MAVLINK_Parameters, streamRates[1], 0), + + // @Param: RC_CHAN + // @DisplayName: RC Channel stream rate to ground station + // @Description: Stream rate of SERVO_OUTPUT_RAW and RC_CHANNELS to ground station + // @Units: Hz + // @Range: 0 10 + // @Increment: 1 + // @User: Advanced + AP_GROUPINFO("RC_CHAN", 2, GCS_MAVLINK_Parameters, streamRates[2], 0), + + // @Param: RAW_CTRL + // @DisplayName: Raw Control stream rate to ground station + // @Description: Stream rate of RC_CHANNELS_SCALED (HIL only) to ground station + // @Units: Hz + // @Range: 0 10 + // @Increment: 1 + // @User: Advanced + AP_GROUPINFO("RAW_CTRL", 3, GCS_MAVLINK_Parameters, streamRates[3], 0), + + // @Param: POSITION + // @DisplayName: Position stream rate to ground station + // @Description: Stream rate of GLOBAL_POSITION_INT and LOCAL_POSITION_NED to ground station + // @Units: Hz + // @Range: 0 10 + // @Increment: 1 + // @User: Advanced + AP_GROUPINFO("POSITION", 4, GCS_MAVLINK_Parameters, streamRates[4], 0), + + // @Param: EXTRA1 + // @DisplayName: Extra data type 1 stream rate to ground station + // @Description: Stream rate of ATTITUDE, SIMSTATE (SITL only), AHRS2 and PID_TUNING to ground station + // @Units: Hz + // @Range: 0 10 + // @Increment: 1 + // @User: Advanced + AP_GROUPINFO("EXTRA1", 5, GCS_MAVLINK_Parameters, streamRates[5], 0), + + // @Param: EXTRA2 + // @DisplayName: Extra data type 2 stream rate to ground station + // @Description: Stream rate of VFR_HUD to ground station + // @Units: Hz + // @Range: 0 10 + // @Increment: 1 + // @User: Advanced + AP_GROUPINFO("EXTRA2", 6, GCS_MAVLINK_Parameters, streamRates[6], 0), + + // @Param: EXTRA3 + // @DisplayName: Extra data type 3 stream rate to ground station + // @Description: Stream rate of AHRS, HWSTATUS, SYSTEM_TIME, RANGEFINDER, DISTANCE_SENSOR, TERRAIN_REQUEST, BATTERY2, MOUNT_STATUS, OPTICAL_FLOW, GIMBAL_REPORT, MAG_CAL_REPORT, MAG_CAL_PROGRESS, EKF_STATUS_REPORT, VIBRATION and RPM to ground station + // @Units: Hz + // @Range: 0 10 + // @Increment: 1 + // @User: Advanced + AP_GROUPINFO("EXTRA3", 7, GCS_MAVLINK_Parameters, streamRates[7], 0), + + // @Param: PARAMS + // @DisplayName: Parameter stream rate to ground station + // @Description: Stream rate of PARAM_VALUE to ground station + // @Units: Hz + // @Range: 0 10 + // @Increment: 1 + // @User: Advanced + AP_GROUPINFO("PARAMS", 8, GCS_MAVLINK_Parameters, streamRates[8], 0), + AP_GROUPEND +}; + +static const ap_message STREAM_RAW_SENSORS_msgs[] = { + MSG_RAW_IMU, + MSG_SCALED_IMU2, + MSG_SCALED_IMU3, + MSG_SCALED_PRESSURE, + MSG_SCALED_PRESSURE2, + MSG_SCALED_PRESSURE3, + MSG_SENSOR_OFFSETS +}; +static const ap_message STREAM_EXTENDED_STATUS_msgs[] = { + MSG_SYS_STATUS, + MSG_POWER_STATUS, + MSG_MEMINFO, + MSG_CURRENT_WAYPOINT, // MISSION_CURRENT + MSG_GPS_RAW, + MSG_GPS_RTK, + MSG_GPS2_RAW, + MSG_GPS2_RTK, + MSG_NAV_CONTROLLER_OUTPUT, + MSG_FENCE_STATUS, + MSG_POSITION_TARGET_GLOBAL_INT, +}; +static const ap_message STREAM_POSITION_msgs[] = { + MSG_LOCATION, + MSG_LOCAL_POSITION +}; +static const ap_message STREAM_RC_CHANNELS_msgs[] = { + MSG_SERVO_OUTPUT_RAW, + MSG_RC_CHANNELS, + MSG_RC_CHANNELS_RAW, // only sent on a mavlink1 connection +}; +static const ap_message STREAM_EXTRA1_msgs[] = { + MSG_ATTITUDE, + MSG_SIMSTATE, + MSG_AHRS2, + MSG_PID_TUNING // Up to four PID_TUNING messages are sent, depending on GCS_PID_MASK parameter +}; +static const ap_message STREAM_EXTRA2_msgs[] = { + MSG_VFR_HUD +}; +static const ap_message STREAM_EXTRA3_msgs[] = { + MSG_AHRS, + MSG_HWSTATUS, + MSG_SYSTEM_TIME, + MSG_WIND, + MSG_RANGEFINDER, + MSG_DISTANCE_SENSOR, + MSG_BATTERY2, + MSG_BATTERY_STATUS, + MSG_MOUNT_STATUS, + MSG_OPTICAL_FLOW, + MSG_GIMBAL_REPORT, + MSG_MAG_CAL_REPORT, + MSG_MAG_CAL_PROGRESS, + MSG_EKF_STATUS_REPORT, + MSG_VIBRATION, + MSG_RPM, + MSG_ESC_TELEMETRY, + MSG_GENERATOR_STATUS, +}; +static const ap_message STREAM_PARAMS_msgs[] = { + MSG_NEXT_PARAM +}; +static const ap_message STREAM_ADSB_msgs[] = { + MSG_ADSB_VEHICLE +}; + +const struct GCS_MAVLINK::stream_entries GCS_MAVLINK::all_stream_entries[] = { + MAV_STREAM_ENTRY(STREAM_RAW_SENSORS), + MAV_STREAM_ENTRY(STREAM_EXTENDED_STATUS), + MAV_STREAM_ENTRY(STREAM_POSITION), + MAV_STREAM_ENTRY(STREAM_RC_CHANNELS), + MAV_STREAM_ENTRY(STREAM_EXTRA1), + MAV_STREAM_ENTRY(STREAM_EXTRA2), + MAV_STREAM_ENTRY(STREAM_EXTRA3), + MAV_STREAM_ENTRY(STREAM_ADSB), + MAV_STREAM_ENTRY(STREAM_PARAMS), + MAV_STREAM_TERMINATOR // must have this at end of stream_entries +}; + +bool GCS_MAVLINK_Blimp::handle_guided_request(AP_Mission::Mission_Command &cmd) +{ + // #if MODE_AUTO_ENABLED == ENABLED + // // return blimp.mode_auto.do_guided(cmd); + // #else + return false; + // #endif +} + +void GCS_MAVLINK_Blimp::handle_change_alt_request(AP_Mission::Mission_Command &cmd) +{ + // add home alt if needed + if (cmd.content.location.relative_alt) { + cmd.content.location.alt += blimp.ahrs.get_home().alt; + } + + // To-Do: update target altitude for loiter or waypoint controller depending upon nav mode +} + +void GCS_MAVLINK_Blimp::packetReceived(const mavlink_status_t &status, + const mavlink_message_t &msg) +{ + GCS_MAVLINK::packetReceived(status, msg); +} + +bool GCS_MAVLINK_Blimp::params_ready() const +{ + if (AP_BoardConfig::in_config_error()) { + // we may never have parameters "initialised" in this case + return true; + } + // if we have not yet initialised (including allocating the motors + // object) we drop this request. That prevents the GCS from getting + // a confusing parameter count during bootup + return blimp.ap.initialised_params; +} + +void GCS_MAVLINK_Blimp::send_banner() +{ + GCS_MAVLINK::send_banner(); + send_text(MAV_SEVERITY_INFO, "Frame: %s", blimp.get_frame_string()); +} + +// a RC override message is considered to be a 'heartbeat' from the ground station for failsafe purposes +void GCS_MAVLINK_Blimp::handle_rc_channels_override(const mavlink_message_t &msg) +{ + blimp.failsafe.last_heartbeat_ms = AP_HAL::millis(); + GCS_MAVLINK::handle_rc_channels_override(msg); +} + +void GCS_MAVLINK_Blimp::handle_command_ack(const mavlink_message_t &msg) +{ + blimp.command_ack_counter++; + GCS_MAVLINK::handle_command_ack(msg); +} + +MAV_RESULT GCS_MAVLINK_Blimp::_handle_command_preflight_calibration(const mavlink_command_long_t &packet) +{ + return GCS_MAVLINK::_handle_command_preflight_calibration(packet); +} + + +MAV_RESULT GCS_MAVLINK_Blimp::handle_command_do_set_roi(const Location &roi_loc) +{ + if (!roi_loc.check_latlng()) { + return MAV_RESULT_FAILED; + } + // blimp.flightmode->auto_yaw.set_roi(roi_loc); + return MAV_RESULT_ACCEPTED; +} + +MAV_RESULT GCS_MAVLINK_Blimp::handle_preflight_reboot(const mavlink_command_long_t &packet) +{ + // call parent + return GCS_MAVLINK::handle_preflight_reboot(packet); +} + +bool GCS_MAVLINK_Blimp::set_home_to_current_location(bool _lock) +{ + return blimp.set_home_to_current_location(_lock); +} +bool GCS_MAVLINK_Blimp::set_home(const Location& loc, bool _lock) +{ + return blimp.set_home(loc, _lock); +} + +MAV_RESULT GCS_MAVLINK_Blimp::handle_command_int_do_reposition(const mavlink_command_int_t &packet) +{ + const bool change_modes = ((int32_t)packet.param2 & MAV_DO_REPOSITION_FLAGS_CHANGE_MODE) == MAV_DO_REPOSITION_FLAGS_CHANGE_MODE; + if (!blimp.flightmode->in_guided_mode() && !change_modes) { + return MAV_RESULT_DENIED; + } + + // sanity check location + if (!check_latlng(packet.x, packet.y)) { + return MAV_RESULT_DENIED; + } + + Location request_location {}; + request_location.lat = packet.x; + request_location.lng = packet.y; + + if (fabsf(packet.z) > LOCATION_ALT_MAX_M) { + return MAV_RESULT_DENIED; + } + + Location::AltFrame frame; + if (!mavlink_coordinate_frame_to_location_alt_frame((MAV_FRAME)packet.frame, frame)) { + return MAV_RESULT_DENIED; // failed as the location is not valid + } + request_location.set_alt_cm((int32_t)(packet.z * 100.0f), frame); + + if (request_location.sanitize(blimp.current_loc)) { + // if the location wasn't already sane don't load it + return MAV_RESULT_DENIED; // failed as the location is not valid + } + + return MAV_RESULT_ACCEPTED; +} + +MAV_RESULT GCS_MAVLINK_Blimp::handle_command_int_packet(const mavlink_command_int_t &packet) +{ + switch (packet.command) { + case MAV_CMD_DO_FOLLOW: + return MAV_RESULT_UNSUPPORTED; + + case MAV_CMD_DO_REPOSITION: + return handle_command_int_do_reposition(packet); + default: + return GCS_MAVLINK::handle_command_int_packet(packet); + } +} + +MAV_RESULT GCS_MAVLINK_Blimp::handle_command_mount(const mavlink_command_long_t &packet) +{ + // if the mount doesn't do pan control then yaw the entire vehicle instead: + switch (packet.command) { + default: + break; + } + return GCS_MAVLINK::handle_command_mount(packet); +} + +MAV_RESULT GCS_MAVLINK_Blimp::handle_command_long_packet(const mavlink_command_long_t &packet) +{ + switch (packet.command) { + + case MAV_CMD_NAV_TAKEOFF: { + // param3 : horizontal navigation by pilot acceptable + // param4 : yaw angle (not supported) + // param5 : latitude (not supported) + // param6 : longitude (not supported) + // param7 : altitude [metres] + + // float takeoff_alt = packet.param7 * 100; // Convert m to cm + + // if (!blimp.flightmode->do_user_takeoff(takeoff_alt, is_zero(packet.param3))) { + // return MAV_RESULT_FAILED; + //MIR Do I need this? + // } + return MAV_RESULT_ACCEPTED; + } + + // #if MODE_AUTO_ENABLED == ENABLED + // case MAV_CMD_DO_LAND_START: + // if (blimp.mode_auto.mission.jump_to_landing_sequence() && blimp.set_mode(Mode::Number::AUTO, ModeReason::GCS_COMMAND)) { + // return MAV_RESULT_ACCEPTED; + // } + // return MAV_RESULT_FAILED; + // #endif + + // case MAV_CMD_NAV_LOITER_UNLIM: + // if (!blimp.set_mode(Mode::Number::LOITER, ModeReason::GCS_COMMAND)) { + // return MAV_RESULT_FAILED; + // } + // return MAV_RESULT_ACCEPTED; + + // case MAV_CMD_NAV_RETURN_TO_LAUNCH: + // if (!blimp.set_mode(Mode::Number::RTL, ModeReason::GCS_COMMAND)) { + // return MAV_RESULT_FAILED; + // } + // return MAV_RESULT_ACCEPTED; + + + case MAV_CMD_CONDITION_YAW: + // param1 : target angle [0-360] + // param2 : speed during change [deg per second] + // param3 : direction (-1:ccw, +1:cw) + // param4 : relative offset (1) or absolute angle (0) + if ((packet.param1 >= 0.0f) && + (packet.param1 <= 360.0f) && + (is_zero(packet.param4) || is_equal(packet.param4,1.0f))) { + // blimp.flightmode->auto_yaw.set_fixed_yaw( + // packet.param1, + // packet.param2, + // (int8_t)packet.param3, + // is_positive(packet.param4)); + return MAV_RESULT_ACCEPTED; + } + return MAV_RESULT_FAILED; + + default: + return GCS_MAVLINK::handle_command_long_packet(packet); + } +} + +void GCS_MAVLINK_Blimp::handleMessage(const mavlink_message_t &msg) +{ + switch (msg.msgid) { + + case MAVLINK_MSG_ID_HEARTBEAT: { // MAV ID: 0 + // We keep track of the last time we received a heartbeat from our GCS for failsafe purposes + if (msg.sysid != blimp.g.sysid_my_gcs) { + break; + } + blimp.failsafe.last_heartbeat_ms = AP_HAL::millis(); + break; + } + + // #if MODE_GUIDED_ENABLED == ENABLED + // case MAVLINK_MSG_ID_SET_ATTITUDE_TARGET: // MAV ID: 82 + // { + // // decode packet + // mavlink_set_attitude_target_t packet; + // mavlink_msg_set_attitude_target_decode(&msg, &packet); + + // // exit if vehicle is not in Guided mode or Auto-Guided mode + // if (!blimp.flightmode->in_guided_mode()) { + // break; + // } + + // // ensure type_mask specifies to use attitude and thrust + // if ((packet.type_mask & ((1<<7)|(1<<6))) != 0) { + // break; + // } + + // // check if the message's thrust field should be interpreted as a climb rate or as thrust + // const bool use_thrust = blimp.g2.dev_options.get() & DevOptionSetAttitudeTarget_ThrustAsThrust; + + // float climb_rate_or_thrust; + // if (use_thrust) { + // // interpret thrust as thrust + // climb_rate_or_thrust = constrain_float(packet.thrust, -1.0f, 1.0f); + // } else { + // // convert thrust to climb rate + // packet.thrust = constrain_float(packet.thrust, 0.0f, 1.0f); + // if (is_equal(packet.thrust, 0.5f)) { + // climb_rate_or_thrust = 0.0f; + // } else if (packet.thrust > 0.5f) { + // // climb at up to WPNAV_SPEED_UP + // climb_rate_or_thrust = (packet.thrust - 0.5f) * 2.0f * blimp.wp_nav->get_default_speed_up(); + // } else { + // // descend at up to WPNAV_SPEED_DN + // climb_rate_or_thrust = (0.5f - packet.thrust) * 2.0f * -fabsf(blimp.wp_nav->get_default_speed_down()); + // } + // } + + // // if the body_yaw_rate field is ignored, use the commanded yaw position + // // otherwise use the commanded yaw rate + // bool use_yaw_rate = false; + // if ((packet.type_mask & (1<<2)) == 0) { + // use_yaw_rate = true; + // } + + // blimp.mode_guided.set_angle(Quaternion(packet.q[0],packet.q[1],packet.q[2],packet.q[3]), + // climb_rate_or_thrust, use_yaw_rate, packet.body_yaw_rate, use_thrust); + + // break; + // } + + // case MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED: // MAV ID: 84 + // { + // // decode packet + // mavlink_set_position_target_local_ned_t packet; + // mavlink_msg_set_position_target_local_ned_decode(&msg, &packet); + + // // exit if vehicle is not in Guided mode or Auto-Guided mode + // if (!blimp.flightmode->in_guided_mode()) { + // break; + // } + + // // check for supported coordinate frames + // if (packet.coordinate_frame != MAV_FRAME_LOCAL_NED && + // packet.coordinate_frame != MAV_FRAME_LOCAL_OFFSET_NED && + // packet.coordinate_frame != MAV_FRAME_BODY_NED && + // packet.coordinate_frame != MAV_FRAME_BODY_OFFSET_NED) { + // break; + // } + + // bool pos_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_POS_IGNORE; + // bool vel_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_VEL_IGNORE; + // bool acc_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_ACC_IGNORE; + // bool yaw_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_YAW_IGNORE; + // bool yaw_rate_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_YAW_RATE_IGNORE; + + // // exit immediately if acceleration provided + // if (!acc_ignore) { + // break; + // } + + // // prepare position + // Vector3f pos_vector; + // if (!pos_ignore) { + // // convert to cm + // pos_vector = Vector3f(packet.x * 100.0f, packet.y * 100.0f, -packet.z * 100.0f); + // // rotate to body-frame if necessary + // if (packet.coordinate_frame == MAV_FRAME_BODY_NED || + // packet.coordinate_frame == MAV_FRAME_BODY_OFFSET_NED) { + // blimp.rotate_body_frame_to_NE(pos_vector.x, pos_vector.y); + // } + // // add body offset if necessary + // if (packet.coordinate_frame == MAV_FRAME_LOCAL_OFFSET_NED || + // packet.coordinate_frame == MAV_FRAME_BODY_NED || + // packet.coordinate_frame == MAV_FRAME_BODY_OFFSET_NED) { + // pos_vector += blimp.inertial_nav.get_position(); + // } + // } + + // // prepare velocity + // Vector3f vel_vector; + // if (!vel_ignore) { + // // convert to cm + // vel_vector = Vector3f(packet.vx * 100.0f, packet.vy * 100.0f, -packet.vz * 100.0f); + // // rotate to body-frame if necessary + // if (packet.coordinate_frame == MAV_FRAME_BODY_NED || packet.coordinate_frame == MAV_FRAME_BODY_OFFSET_NED) { + // blimp.rotate_body_frame_to_NE(vel_vector.x, vel_vector.y); + // } + // } + + // // prepare yaw + // float yaw_cd = 0.0f; + // bool yaw_relative = false; + // float yaw_rate_cds = 0.0f; + // if (!yaw_ignore) { + // yaw_cd = ToDeg(packet.yaw) * 100.0f; + // yaw_relative = packet.coordinate_frame == MAV_FRAME_BODY_NED || packet.coordinate_frame == MAV_FRAME_BODY_OFFSET_NED; + // } + // if (!yaw_rate_ignore) { + // yaw_rate_cds = ToDeg(packet.yaw_rate) * 100.0f; + // } + + // // send request + // if (!pos_ignore && !vel_ignore) { + // blimp.mode_guided.set_destination_posvel(pos_vector, vel_vector, !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds, yaw_relative); + // } else if (pos_ignore && !vel_ignore) { + // blimp.mode_guided.set_velocity(vel_vector, !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds, yaw_relative); + // } else if (!pos_ignore && vel_ignore) { + // blimp.mode_guided.set_destination(pos_vector, !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds, yaw_relative); + // } + + // break; + // } + + // case MAVLINK_MSG_ID_SET_POSITION_TARGET_GLOBAL_INT: // MAV ID: 86 + // { + // // decode packet + // mavlink_set_position_target_global_int_t packet; + // mavlink_msg_set_position_target_global_int_decode(&msg, &packet); + + // // exit if vehicle is not in Guided mode or Auto-Guided mode + // if (!blimp.flightmode->in_guided_mode()) { + // break; + // } + + // bool pos_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_POS_IGNORE; + // bool vel_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_VEL_IGNORE; + // bool acc_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_ACC_IGNORE; + // bool yaw_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_YAW_IGNORE; + // bool yaw_rate_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_YAW_RATE_IGNORE; + + // // exit immediately if acceleration provided + // if (!acc_ignore) { + // break; + // } + + // // extract location from message + // Location loc; + // if (!pos_ignore) { + // // sanity check location + // if (!check_latlng(packet.lat_int, packet.lon_int)) { + // break; + // } + // Location::AltFrame frame; + // if (!mavlink_coordinate_frame_to_location_alt_frame((MAV_FRAME)packet.coordinate_frame, frame)) { + // // unknown coordinate frame + // break; + // } + // loc = {packet.lat_int, packet.lon_int, int32_t(packet.alt*100), frame}; + // } + + // // prepare yaw + // float yaw_cd = 0.0f; + // bool yaw_relative = false; + // float yaw_rate_cds = 0.0f; + // if (!yaw_ignore) { + // yaw_cd = ToDeg(packet.yaw) * 100.0f; + // yaw_relative = packet.coordinate_frame == MAV_FRAME_BODY_NED || packet.coordinate_frame == MAV_FRAME_BODY_OFFSET_NED; + // } + // if (!yaw_rate_ignore) { + // yaw_rate_cds = ToDeg(packet.yaw_rate) * 100.0f; + // } + + // // send targets to the appropriate guided mode controller + // if (!pos_ignore && !vel_ignore) { + // // convert Location to vector from ekf origin for posvel controller + // if (loc.get_alt_frame() == Location::AltFrame::ABOVE_TERRAIN) { + // // posvel controller does not support alt-above-terrain + // break; + // } + // Vector3f pos_neu_cm; + // if (!loc.get_vector_from_origin_NEU(pos_neu_cm)) { + // break; + // } + // blimp.mode_guided.set_destination_posvel(pos_neu_cm, Vector3f(packet.vx * 100.0f, packet.vy * 100.0f, -packet.vz * 100.0f), !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds, yaw_relative); + // } else if (pos_ignore && !vel_ignore) { + // blimp.mode_guided.set_velocity(Vector3f(packet.vx * 100.0f, packet.vy * 100.0f, -packet.vz * 100.0f), !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds, yaw_relative); + // } else if (!pos_ignore && vel_ignore) { + // blimp.mode_guided.set_destination(loc, !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds, yaw_relative); + // } + + // break; + // } + // #endif + + case MAVLINK_MSG_ID_RADIO: + case MAVLINK_MSG_ID_RADIO_STATUS: { // MAV ID: 109 + handle_radio_status(msg, blimp.should_log(MASK_LOG_PM)); + break; + } + + case MAVLINK_MSG_ID_TERRAIN_DATA: + case MAVLINK_MSG_ID_TERRAIN_CHECK: + break; + + case MAVLINK_MSG_ID_SET_HOME_POSITION: { + mavlink_set_home_position_t packet; + mavlink_msg_set_home_position_decode(&msg, &packet); + if ((packet.latitude == 0) && (packet.longitude == 0) && (packet.altitude == 0)) { + if (!blimp.set_home_to_current_location(true)) { + // silently ignored + } + } else { + Location new_home_loc; + new_home_loc.lat = packet.latitude; + new_home_loc.lng = packet.longitude; + new_home_loc.alt = packet.altitude / 10; + if (!blimp.set_home(new_home_loc, true)) { + // silently ignored + } + } + break; + } + + case MAVLINK_MSG_ID_ADSB_VEHICLE: + case MAVLINK_MSG_ID_UAVIONIX_ADSB_OUT_CFG: + case MAVLINK_MSG_ID_UAVIONIX_ADSB_OUT_DYNAMIC: + case MAVLINK_MSG_ID_UAVIONIX_ADSB_TRANSCEIVER_HEALTH_REPORT: + break; + + default: + handle_common_message(msg); + break; + } // end switch +} // end handle mavlink + + +MAV_RESULT GCS_MAVLINK_Blimp::handle_flight_termination(const mavlink_command_long_t &packet) +{ + MAV_RESULT result = MAV_RESULT_FAILED; + if (packet.param1 > 0.5f) { + blimp.arming.disarm(AP_Arming::Method::TERMINATION); + result = MAV_RESULT_ACCEPTED; + } + return result; +} + +float GCS_MAVLINK_Blimp::vfr_hud_alt() const +{ + if (blimp.g2.dev_options.get() & DevOptionVFR_HUDRelativeAlt) { + // compatibility option for older mavlink-aware devices that + // assume Blimp returns a relative altitude in VFR_HUD.alt + return blimp.current_loc.alt * 0.01f; + } + return GCS_MAVLINK::vfr_hud_alt(); +} + +uint64_t GCS_MAVLINK_Blimp::capabilities() const +{ + return (MAV_PROTOCOL_CAPABILITY_MISSION_FLOAT | + MAV_PROTOCOL_CAPABILITY_MISSION_INT | + MAV_PROTOCOL_CAPABILITY_COMMAND_INT | + MAV_PROTOCOL_CAPABILITY_SET_POSITION_TARGET_LOCAL_NED | + MAV_PROTOCOL_CAPABILITY_SET_POSITION_TARGET_GLOBAL_INT | + MAV_PROTOCOL_CAPABILITY_FLIGHT_TERMINATION | + MAV_PROTOCOL_CAPABILITY_SET_ATTITUDE_TARGET | + GCS_MAVLINK::capabilities()); +} + +MAV_LANDED_STATE GCS_MAVLINK_Blimp::landed_state() const +{ + if (blimp.ap.land_complete) { + return MAV_LANDED_STATE_ON_GROUND; + } + if (blimp.flightmode->is_landing()) { + return MAV_LANDED_STATE_LANDING; + } + // if (blimp.flightmode->is_taking_off()) { + // return MAV_LANDED_STATE_TAKEOFF; + // } + return MAV_LANDED_STATE_IN_AIR; +} + +void GCS_MAVLINK_Blimp::send_wind() const +{ + Vector3f airspeed_vec_bf; + if (!AP::ahrs().airspeed_vector_true(airspeed_vec_bf)) { + // if we don't have an airspeed estimate then we don't have a + // valid wind estimate on blimps + return; + } + const Vector3f wind = AP::ahrs().wind_estimate(); + mavlink_msg_wind_send( + chan, + degrees(atan2f(-wind.y, -wind.x)), + wind.length(), + wind.z); +} diff --git a/Blimp/GCS_Mavlink.h b/Blimp/GCS_Mavlink.h new file mode 100644 index 0000000000..4ae1ba3057 --- /dev/null +++ b/Blimp/GCS_Mavlink.h @@ -0,0 +1,71 @@ +#pragma once + +#include + +class GCS_MAVLINK_Blimp : public GCS_MAVLINK +{ + +public: + + using GCS_MAVLINK::GCS_MAVLINK; + +protected: + + uint32_t telem_delay() const override; + + MAV_RESULT handle_flight_termination(const mavlink_command_long_t &packet) override; + + uint8_t sysid_my_gcs() const override; + bool sysid_enforce() const override; + + bool params_ready() const override; + void send_banner() override; + + MAV_RESULT _handle_command_preflight_calibration(const mavlink_command_long_t &packet) override; + + void send_position_target_global_int() override; + // void send_position_target_local_ned() override; + + MAV_RESULT handle_command_do_set_roi(const Location &roi_loc) override; + MAV_RESULT handle_preflight_reboot(const mavlink_command_long_t &packet) override; + MAV_RESULT handle_command_mount(const mavlink_command_long_t &packet) override; + MAV_RESULT handle_command_int_packet(const mavlink_command_int_t &packet) override; + MAV_RESULT handle_command_long_packet(const mavlink_command_long_t &packet) override; + MAV_RESULT handle_command_int_do_reposition(const mavlink_command_int_t &packet); + + // void handle_mount_message(const mavlink_message_t &msg) override; + + bool set_home_to_current_location(bool lock) override WARN_IF_UNUSED; + bool set_home(const Location& loc, bool lock) override WARN_IF_UNUSED; + void send_nav_controller_output() const override; //MIR can't comment out or build fails?? + uint64_t capabilities() const override; + + virtual MAV_VTOL_STATE vtol_state() const override + { + return MAV_VTOL_STATE_MC; + }; + virtual MAV_LANDED_STATE landed_state() const override; + +private: + + void handleMessage(const mavlink_message_t &msg) override; + void handle_command_ack(const mavlink_message_t &msg) override; + bool handle_guided_request(AP_Mission::Mission_Command &cmd) override; + void handle_change_alt_request(AP_Mission::Mission_Command &cmd) override; + void handle_rc_channels_override(const mavlink_message_t &msg) override; + bool try_send_message(enum ap_message id) override; + + void packetReceived(const mavlink_status_t &status, + const mavlink_message_t &msg) override; + + MAV_MODE base_mode() const override; + MAV_STATE vehicle_system_status() const override; + + float vfr_hud_airspeed() const override; + int16_t vfr_hud_throttle() const override; + float vfr_hud_alt() const override; + + void send_pid_tuning() override; + + void send_wind() const; +}; diff --git a/Blimp/Log.cpp b/Blimp/Log.cpp new file mode 100644 index 0000000000..d7ca4d2e41 --- /dev/null +++ b/Blimp/Log.cpp @@ -0,0 +1,555 @@ +#include "Blimp.h" + +#if LOGGING_ENABLED == ENABLED + +// Code to Write and Read packets from AP_Logger log memory +// Code to interact with the user to dump or erase logs + +struct PACKED log_Control_Tuning { + LOG_PACKET_HEADER; + uint64_t time_us; + float throttle_in; + float angle_boost; + float throttle_out; + float throttle_hover; + float desired_alt; + float inav_alt; + int32_t baro_alt; + float desired_rangefinder_alt; + float rangefinder_alt; + float terr_alt; + int16_t target_climb_rate; + int16_t climb_rate; +}; + +// Write a control tuning packet +void Blimp::Log_Write_Control_Tuning() +{ + // get terrain altitude + // float terr_alt = 0.0f; + // float des_alt_m = 0.0f; + // int16_t target_climb_rate_cms = 0; + // if (!flightmode->has_manual_throttle()) { + // des_alt_m = pos_control->get_alt_target() / 100.0f; + // target_climb_rate_cms = pos_control->get_vel_target_z(); + // } + + // get surface tracking alts + // float desired_rangefinder_alt; + // if (!surface_tracking.get_target_dist_for_logging(desired_rangefinder_alt)) { + // desired_rangefinder_alt = AP::logger().quiet_nan(); + // } + + // struct log_Control_Tuning pkt = { + // LOG_PACKET_HEADER_INIT(LOG_CONTROL_TUNING_MSG), + // time_us : AP_HAL::micros64(), + // throttle_in : attitude_control->get_throttle_in(), + // angle_boost : attitude_control->angle_boost(), + // throttle_out : motors->get_throttle(), + // throttle_hover : motors->get_throttle_hover(), + // desired_alt : des_alt_m, + // inav_alt : inertial_nav.get_altitude() / 100.0f, + // baro_alt : baro_alt, + // desired_rangefinder_alt : desired_rangefinder_alt, + // rangefinder_alt : surface_tracking.get_dist_for_logging(), + // terr_alt : terr_alt, + // target_climb_rate : target_climb_rate_cms, + // climb_rate : int16_t(inertial_nav.get_velocity_z()) // float -> int16_t + // }; + // logger.WriteBlock(&pkt, sizeof(pkt)); +} + +// Write an attitude packet +void Blimp::Log_Write_Attitude() +{ + // // Vector3f targets = attitude_control->get_att_target_euler_cd(); + // targets.z = wrap_360_cd(targets.z); + // logger.Write_Attitude(targets); + // logger.Write_Rate(ahrs_view, *motors, *attitude_control, *pos_control); + // if (should_log(MASK_LOG_PID)) { + // logger.Write_PID(LOG_PIDR_MSG, attitude_control->get_rate_roll_pid().get_pid_info()); + // logger.Write_PID(LOG_PIDP_MSG, attitude_control->get_rate_pitch_pid().get_pid_info()); + // logger.Write_PID(LOG_PIDY_MSG, attitude_control->get_rate_yaw_pid().get_pid_info()); + // // logger.Write_PID(LOG_PIDA_MSG, pos_control->get_accel_z_pid().get_pid_info() ); + // } +} + +// Write an EKF and POS packet +void Blimp::Log_Write_EKF_POS() +{ + AP::ahrs_navekf().Log_Write(); + ahrs.Write_AHRS2(); +#if CONFIG_HAL_BOARD == HAL_BOARD_SITL + sitl.Log_Write_SIMSTATE(); +#endif + ahrs.Write_POS(); +} + +struct PACKED log_MotBatt { + LOG_PACKET_HEADER; + uint64_t time_us; + float lift_max; + float bat_volt; + float bat_res; + float th_limit; +}; + +// Write an rate packet +void Blimp::Log_Write_MotBatt() +{ + // struct log_MotBatt pkt_mot = { + // LOG_PACKET_HEADER_INIT(LOG_MOTBATT_MSG), + // time_us : AP_HAL::micros64(), + // lift_max : (float)(motors->get_lift_max()), + // bat_volt : (float)(motors->get_batt_voltage_filt()), + // bat_res : (float)(battery.get_resistance()), + // th_limit : (float)(motors->get_throttle_limit()) + // }; + // logger.WriteBlock(&pkt_mot, sizeof(pkt_mot)); +} + +struct PACKED log_Data_Int16t { + LOG_PACKET_HEADER; + uint64_t time_us; + uint8_t id; + int16_t data_value; +}; + +// Write an int16_t data packet +UNUSED_FUNCTION +void Blimp::Log_Write_Data(LogDataID id, int16_t value) +{ + if (should_log(MASK_LOG_ANY)) { + struct log_Data_Int16t pkt = { + LOG_PACKET_HEADER_INIT(LOG_DATA_INT16_MSG), +time_us : AP_HAL::micros64(), +id : (uint8_t)id, +data_value : value + }; + logger.WriteCriticalBlock(&pkt, sizeof(pkt)); + } +} + +struct PACKED log_Data_UInt16t { + LOG_PACKET_HEADER; + uint64_t time_us; + uint8_t id; + uint16_t data_value; +}; + +// Write an uint16_t data packet +UNUSED_FUNCTION +void Blimp::Log_Write_Data(LogDataID id, uint16_t value) +{ + if (should_log(MASK_LOG_ANY)) { + struct log_Data_UInt16t pkt = { + LOG_PACKET_HEADER_INIT(LOG_DATA_UINT16_MSG), +time_us : AP_HAL::micros64(), +id : (uint8_t)id, +data_value : value + }; + logger.WriteCriticalBlock(&pkt, sizeof(pkt)); + } +} + +struct PACKED log_Data_Int32t { + LOG_PACKET_HEADER; + uint64_t time_us; + uint8_t id; + int32_t data_value; +}; + +// Write an int32_t data packet +void Blimp::Log_Write_Data(LogDataID id, int32_t value) +{ + if (should_log(MASK_LOG_ANY)) { + struct log_Data_Int32t pkt = { + LOG_PACKET_HEADER_INIT(LOG_DATA_INT32_MSG), +time_us : AP_HAL::micros64(), +id : (uint8_t)id, +data_value : value + }; + logger.WriteCriticalBlock(&pkt, sizeof(pkt)); + } +} + +struct PACKED log_Data_UInt32t { + LOG_PACKET_HEADER; + uint64_t time_us; + uint8_t id; + uint32_t data_value; +}; + +// Write a uint32_t data packet +void Blimp::Log_Write_Data(LogDataID id, uint32_t value) +{ + if (should_log(MASK_LOG_ANY)) { + struct log_Data_UInt32t pkt = { + LOG_PACKET_HEADER_INIT(LOG_DATA_UINT32_MSG), +time_us : AP_HAL::micros64(), +id : (uint8_t)id, +data_value : value + }; + logger.WriteCriticalBlock(&pkt, sizeof(pkt)); + } +} + +struct PACKED log_Data_Float { + LOG_PACKET_HEADER; + uint64_t time_us; + uint8_t id; + float data_value; +}; + +// Write a float data packet +UNUSED_FUNCTION +void Blimp::Log_Write_Data(LogDataID id, float value) +{ + if (should_log(MASK_LOG_ANY)) { + struct log_Data_Float pkt = { + LOG_PACKET_HEADER_INIT(LOG_DATA_FLOAT_MSG), +time_us : AP_HAL::micros64(), +id : (uint8_t)id, +data_value : value + }; + logger.WriteCriticalBlock(&pkt, sizeof(pkt)); + } +} + +struct PACKED log_ParameterTuning { + LOG_PACKET_HEADER; + uint64_t time_us; + uint8_t parameter; // parameter we are tuning, e.g. 39 is CH6_CIRCLE_RATE + float tuning_value; // normalized value used inside tuning() function + float tuning_min; // tuning minimum value + float tuning_max; // tuning maximum value +}; + +void Blimp::Log_Write_Parameter_Tuning(uint8_t param, float tuning_val, float tune_min, float tune_max) +{ + struct log_ParameterTuning pkt_tune = { + LOG_PACKET_HEADER_INIT(LOG_PARAMTUNE_MSG), +time_us : AP_HAL::micros64(), +parameter : param, +tuning_value : tuning_val, +tuning_min : tune_min, +tuning_max : tune_max + }; + + logger.WriteBlock(&pkt_tune, sizeof(pkt_tune)); +} + +// logs when baro or compass becomes unhealthy +void Blimp::Log_Sensor_Health() +{ + // check baro + if (sensor_health.baro != barometer.healthy()) { + sensor_health.baro = barometer.healthy(); + AP::logger().Write_Error(LogErrorSubsystem::BARO, + (sensor_health.baro ? LogErrorCode::ERROR_RESOLVED : LogErrorCode::UNHEALTHY)); + } + + // check compass + if (sensor_health.compass != compass.healthy()) { + sensor_health.compass = compass.healthy(); + AP::logger().Write_Error(LogErrorSubsystem::COMPASS, (sensor_health.compass ? LogErrorCode::ERROR_RESOLVED : LogErrorCode::UNHEALTHY)); + } + + // check primary GPS + if (sensor_health.primary_gps != gps.primary_sensor()) { + sensor_health.primary_gps = gps.primary_sensor(); + AP::logger().Write_Event(LogEvent::GPS_PRIMARY_CHANGED); + } +} + +struct PACKED log_SysIdD { + LOG_PACKET_HEADER; + uint64_t time_us; + float waveform_time; + float waveform_sample; + float waveform_freq; + float angle_x; + float angle_y; + float angle_z; + float accel_x; + float accel_y; + float accel_z; +}; + +// Write an rate packet +void Blimp::Log_Write_SysID_Data(float waveform_time, float waveform_sample, float waveform_freq, float angle_x, float angle_y, float angle_z, float accel_x, float accel_y, float accel_z) +{ +#if MODE_SYSTEMID_ENABLED == ENABLED + struct log_SysIdD pkt_sidd = { + LOG_PACKET_HEADER_INIT(LOG_SYSIDD_MSG), +time_us : AP_HAL::micros64(), +waveform_time : waveform_time, +waveform_sample : waveform_sample, +waveform_freq : waveform_freq, +angle_x : angle_x, +angle_y : angle_y, +angle_z : angle_z, +accel_x : accel_x, +accel_y : accel_y, +accel_z : accel_z + }; + logger.WriteBlock(&pkt_sidd, sizeof(pkt_sidd)); +#endif +} + +struct PACKED log_SysIdS { + LOG_PACKET_HEADER; + uint64_t time_us; + uint8_t systemID_axis; + float waveform_magnitude; + float frequency_start; + float frequency_stop; + float time_fade_in; + float time_const_freq; + float time_record; + float time_fade_out; +}; + +// Write an rate packet +void Blimp::Log_Write_SysID_Setup(uint8_t systemID_axis, float waveform_magnitude, float frequency_start, float frequency_stop, float time_fade_in, float time_const_freq, float time_record, float time_fade_out) +{ +#if MODE_SYSTEMID_ENABLED == ENABLED + struct log_SysIdS pkt_sids = { + LOG_PACKET_HEADER_INIT(LOG_SYSIDS_MSG), +time_us : AP_HAL::micros64(), +systemID_axis : systemID_axis, +waveform_magnitude : waveform_magnitude, +frequency_start : frequency_start, +frequency_stop : frequency_stop, +time_fade_in : time_fade_in, +time_const_freq : time_const_freq, +time_record : time_record, +time_fade_out : time_fade_out + }; + logger.WriteBlock(&pkt_sids, sizeof(pkt_sids)); +#endif +} + +// // guided target logging +// struct PACKED log_GuidedTarget { +// LOG_PACKET_HEADER; +// uint64_t time_us; +// uint8_t type; +// float pos_target_x; +// float pos_target_y; +// float pos_target_z; +// float vel_target_x; +// float vel_target_y; +// float vel_target_z; +// }; + +// Write a Guided mode target +// pos_target is lat, lon, alt OR offset from ekf origin in cm OR roll, pitch, yaw target in centi-degrees +// vel_target is cm/s +// void Blimp::Log_Write_GuidedTarget(uint8_t target_type, const Vector3f& pos_target, const Vector3f& vel_target) +// { +// struct log_GuidedTarget pkt = { +// LOG_PACKET_HEADER_INIT(LOG_GUIDEDTARGET_MSG), +// time_us : AP_HAL::micros64(), +// type : target_type, +// pos_target_x : pos_target.x, +// pos_target_y : pos_target.y, +// pos_target_z : pos_target.z, +// vel_target_x : vel_target.x, +// vel_target_y : vel_target.y, +// vel_target_z : vel_target.z +// }; +// logger.WriteBlock(&pkt, sizeof(pkt)); +// } + +// type and unit information can be found in +// libraries/AP_Logger/Logstructure.h; search for "log_Units" for +// units and "Format characters" for field type information +const struct LogStructure Blimp::log_structure[] = { + LOG_COMMON_STRUCTURES, + + // @LoggerMessage: PTUN + // @Description: Parameter Tuning information + // @URL: https://ardupilot.org/blimp/docs/tuning.html#in-flight-tuning + // @Field: TimeUS: Time since system startup + // @Field: Param: Parameter being tuned + // @Field: TunVal: Normalized value used inside tuning() function + // @Field: TunMin: Tuning minimum limit + // @Field: TunMax: Tuning maximum limit + + { + LOG_PARAMTUNE_MSG, sizeof(log_ParameterTuning), + "PTUN", "QBfff", "TimeUS,Param,TunVal,TunMin,TunMax", "s----", "F----" + }, + + // @LoggerMessage: CTUN + // @Description: Control Tuning information + // @Field: TimeUS: Time since system startup + // @Field: ThI: throttle input + // @Field: ABst: angle boost + // @Field: ThO: throttle output + // @Field: ThH: calculated hover throttle + // @Field: DAlt: desired altitude + // @Field: Alt: achieved altitude + // @Field: BAlt: barometric altitude + // @Field: DSAlt: desired rangefinder altitude + // @Field: SAlt: achieved rangefinder altitude + // @Field: TAlt: terrain altitude + // @Field: DCRt: desired climb rate + // @Field: CRt: climb rate + + // @LoggerMessage: D16 + // @Description: Generic 16-bit-signed-integer storage + // @Field: TimeUS: Time since system startup + // @Field: Id: Data type identifier + // @Field: Value: Value + + // @LoggerMessage: DU16 + // @Description: Generic 16-bit-unsigned-integer storage + // @Field: TimeUS: Time since system startup + // @Field: Id: Data type identifier + // @Field: Value: Value + + // @LoggerMessage: D32 + // @Description: Generic 32-bit-signed-integer storage + // @Field: TimeUS: Time since system startup + // @Field: Id: Data type identifier + // @Field: Value: Value + + // @LoggerMessage: DFLT + // @Description: Generic float storage + // @Field: TimeUS: Time since system startup + // @Field: Id: Data type identifier + // @Field: Value: Value + + // @LoggerMessage: DU32 + // @Description: Generic 32-bit-unsigned-integer storage + // @Field: TimeUS: Time since system startup + // @Field: Id: Data type identifier + // @Field: Value: Value + + { + LOG_CONTROL_TUNING_MSG, sizeof(log_Control_Tuning), + "CTUN", "Qffffffefffhh", "TimeUS,ThI,ABst,ThO,ThH,DAlt,Alt,BAlt,DSAlt,SAlt,TAlt,DCRt,CRt", "s----mmmmmmnn", "F----00B000BB" + }, + + // @LoggerMessage: MOTB + // @Description: Battery information + // @Field: TimeUS: Time since system startup + // @Field: LiftMax: Maximum motor compensation gain + // @Field: BatVolt: Ratio betwen detected battery voltage and maximum battery voltage + // @Field: BatRes: Estimated battery resistance + // @Field: ThLimit: Throttle limit set due to battery current limitations + + { + LOG_MOTBATT_MSG, sizeof(log_MotBatt), + "MOTB", "Qffff", "TimeUS,LiftMax,BatVolt,BatRes,ThLimit", "s-vw-", "F-00-" + }, + { + LOG_DATA_INT16_MSG, sizeof(log_Data_Int16t), + "D16", "QBh", "TimeUS,Id,Value", "s--", "F--" + }, + { + LOG_DATA_UINT16_MSG, sizeof(log_Data_UInt16t), + "DU16", "QBH", "TimeUS,Id,Value", "s--", "F--" + }, + { + LOG_DATA_INT32_MSG, sizeof(log_Data_Int32t), + "D32", "QBi", "TimeUS,Id,Value", "s--", "F--" + }, + { + LOG_DATA_UINT32_MSG, sizeof(log_Data_UInt32t), + "DU32", "QBI", "TimeUS,Id,Value", "s--", "F--" + }, + { + LOG_DATA_FLOAT_MSG, sizeof(log_Data_Float), + "DFLT", "QBf", "TimeUS,Id,Value", "s--", "F--" + }, + + // @LoggerMessage: SIDD + // @Description: System ID data + // @Field: TimeUS: Time since system startup + // @Field: Time: Time reference for waveform + // @Field: Targ: Current waveform sample + // @Field: F: Instantaneous waveform frequency + // @Field: Gx: Delta angle, X-Axis + // @Field: Gy: Delta angle, Y-Axis + // @Field: Gz: Delta angle, Z-Axis + // @Field: Ax: Delta velocity, X-Axis + // @Field: Ay: Delta velocity, Y-Axis + // @Field: Az: Delta velocity, Z-Axis + + { + LOG_SYSIDD_MSG, sizeof(log_SysIdD), + "SIDD", "Qfffffffff", "TimeUS,Time,Targ,F,Gx,Gy,Gz,Ax,Ay,Az", "ss-zkkkooo", "F---------" + }, + + // @LoggerMessage: SIDS + // @Description: System ID settings + // @Field: TimeUS: Time since system startup + // @Field: Ax: The axis which is being excited + // @Field: Mag: Magnitude of the chirp waveform + // @Field: FSt: Frequency at the start of chirp + // @Field: FSp: Frequency at the end of chirp + // @Field: TFin: Time to reach maximum amplitude of chirp + // @Field: TC: Time at constant frequency before chirp starts + // @Field: TR: Time taken to complete chirp waveform + // @Field: TFout: Time to reach zero amplitude after chirp finishes + + { + LOG_SYSIDS_MSG, sizeof(log_SysIdS), + "SIDS", "QBfffffff", "TimeUS,Ax,Mag,FSt,FSp,TFin,TC,TR,TFout", "s--ssssss", "F--------" + }, + + // // @LoggerMessage: GUID + // // @Description: Guided mode target information + // // @Field: TimeUS: Time since system startup + // // @Field: Type: Type of guided mode + // // @Field: pX: Target position, X-Axis + // // @Field: pY: Target position, Y-Axis + // // @Field: pZ: Target position, Z-Axis + // // @Field: vX: Target velocity, X-Axis + // // @Field: vY: Target velocity, Y-Axis + // // @Field: vZ: Target velocity, Z-Axis + + // { LOG_GUIDEDTARGET_MSG, sizeof(log_GuidedTarget), + // "GUID", "QBffffff", "TimeUS,Type,pX,pY,pZ,vX,vY,vZ", "s-mmmnnn", "F-BBBBBB" }, +}; + +void Blimp::Log_Write_Vehicle_Startup_Messages() +{ + // only 200(?) bytes are guaranteed by AP_Logger + logger.Write_MessageF("Frame: %s", get_frame_string()); + logger.Write_Mode((uint8_t)control_mode, control_mode_reason); + ahrs.Log_Write_Home_And_Origin(); + gps.Write_AP_Logger_Log_Startup_messages(); +} + +void Blimp::log_init(void) +{ + logger.Init(log_structure, ARRAY_SIZE(log_structure)); +} + +#else // LOGGING_ENABLED + +void Blimp::Log_Write_Control_Tuning() {} +void Blimp::Log_Write_Performance() {} +void Blimp::Log_Write_Attitude(void) {} +void Blimp::Log_Write_EKF_POS() {} +void Blimp::Log_Write_MotBatt() {} +void Blimp::Log_Write_Data(LogDataID id, int32_t value) {} +void Blimp::Log_Write_Data(LogDataID id, uint32_t value) {} +void Blimp::Log_Write_Data(LogDataID id, int16_t value) {} +void Blimp::Log_Write_Data(LogDataID id, uint16_t value) {} +void Blimp::Log_Write_Data(LogDataID id, float value) {} +void Blimp::Log_Write_Parameter_Tuning(uint8_t param, float tuning_val, float tune_min, float tune_max) {} +void Blimp::Log_Sensor_Health() {} +void Blimp::Log_Write_Precland() {} +void Blimp::Log_Write_GuidedTarget(uint8_t target_type, const Vector3f& pos_target, const Vector3f& vel_target) {} +void Blimp::Log_Write_SysID_Setup(uint8_t systemID_axis, float waveform_magnitude, float frequency_start, float frequency_stop, float time_fade_in, float time_const_freq, float time_record, float time_fade_out) {} +void Blimp::Log_Write_SysID_Data(float waveform_time, float waveform_sample, float waveform_freq, float angle_x, float angle_y, float angle_z, float accel_x, float accel_y, float accel_z) {} +void Blimp::Log_Write_Vehicle_Startup_Messages() {} + +void Blimp::log_init(void) {} + +#endif // LOGGING_ENABLED diff --git a/Blimp/Parameters.cpp b/Blimp/Parameters.cpp new file mode 100644 index 0000000000..62eeacef5a --- /dev/null +++ b/Blimp/Parameters.cpp @@ -0,0 +1,727 @@ +#include "Blimp.h" + +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ + +/* + * Blimp parameter definitions + * + */ + +#define GSCALAR(v, name, def) { blimp.g.v.vtype, name, Parameters::k_param_ ## v, &blimp.g.v, {def_value : def} } +#define ASCALAR(v, name, def) { blimp.aparm.v.vtype, name, Parameters::k_param_ ## v, (const void *)&blimp.aparm.v, {def_value : def} } +#define GGROUP(v, name, class) { AP_PARAM_GROUP, name, Parameters::k_param_ ## v, &blimp.g.v, {group_info : class::var_info} } +#define GOBJECT(v, name, class) { AP_PARAM_GROUP, name, Parameters::k_param_ ## v, (const void *)&blimp.v, {group_info : class::var_info} } +#define GOBJECTPTR(v, name, class) { AP_PARAM_GROUP, name, Parameters::k_param_ ## v, (const void *)&blimp.v, {group_info : class::var_info}, AP_PARAM_FLAG_POINTER } +#define GOBJECTVARPTR(v, name, var_info_ptr) { AP_PARAM_GROUP, name, Parameters::k_param_ ## v, (const void *)&blimp.v, {group_info_ptr : var_info_ptr}, AP_PARAM_FLAG_POINTER | AP_PARAM_FLAG_INFO_POINTER } +#define GOBJECTN(v, pname, name, class) { AP_PARAM_GROUP, name, Parameters::k_param_ ## pname, (const void *)&blimp.v, {group_info : class::var_info} } + +#define DEFAULT_FRAME_CLASS 0 + +const AP_Param::Info Blimp::var_info[] = { + // @Param: FORMAT_VERSION + // @DisplayName: Eeprom format version number + // @Description: This value is incremented when changes are made to the eeprom format + // @User: Advanced + // @ReadOnly: True + GSCALAR(format_version, "FORMAT_VERSION", 0), + + // @Param: SYSID_THISMAV + // @DisplayName: MAVLink system ID of this vehicle + // @Description: Allows setting an individual MAVLink system id for this vehicle to distinguish it from others on the same network + // @Range: 1 255 + // @User: Advanced + GSCALAR(sysid_this_mav, "SYSID_THISMAV", MAV_SYSTEM_ID), + + // @Param: SYSID_MYGCS + // @DisplayName: My ground station number + // @Description: Allows restricting radio overrides to only come from my ground station + // @Range: 1 255 + // @User: Advanced + GSCALAR(sysid_my_gcs, "SYSID_MYGCS", 255), + + // @Param: PILOT_THR_FILT + // @DisplayName: Throttle filter cutoff + // @Description: Throttle filter cutoff (Hz) - active whenever altitude control is inactive - 0 to disable + // @User: Advanced + // @Units: Hz + // @Range: 0 10 + // @Increment: .5 + GSCALAR(throttle_filt, "PILOT_THR_FILT", 0), + + // @Param: PILOT_TKOFF_ALT + // @DisplayName: Pilot takeoff altitude + // @Description: Altitude that altitude control modes will climb to when a takeoff is triggered with the throttle stick. + // @User: Standard + // @Units: cm + // @Range: 0.0 1000.0 + // @Increment: 10 + GSCALAR(pilot_takeoff_alt, "PILOT_TKOFF_ALT", PILOT_TKOFF_ALT_DEFAULT), + + // @Param: PILOT_THR_BHV + // @DisplayName: Throttle stick behavior + // @Description: Bitmask containing various throttle stick options. TX with sprung throttle can set PILOT_THR_BHV to "1" so motor feedback when landed starts from mid-stick instead of bottom of stick. + // @User: Standard + // @Values: 0:None,1:Feedback from mid stick,2:High throttle cancels landing,4:Disarm on land detection + // @Bitmask: 0:Feedback from mid stick,1:High throttle cancels landing,2:Disarm on land detection + GSCALAR(throttle_behavior, "PILOT_THR_BHV", 0), + + // @Group: SERIAL + // @Path: ../libraries/AP_SerialManager/AP_SerialManager.cpp + GOBJECT(serial_manager, "SERIAL", AP_SerialManager), + + // @Param: TELEM_DELAY + // @DisplayName: Telemetry startup delay + // @Description: The amount of time (in seconds) to delay radio telemetry to prevent an Xbee bricking on power up + // @User: Advanced + // @Units: s + // @Range: 0 30 + // @Increment: 1 + GSCALAR(telem_delay, "TELEM_DELAY", 0), + + // @Param: GCS_PID_MASK + // @DisplayName: GCS PID tuning mask + // @Description: bitmask of PIDs to send MAVLink PID_TUNING messages for + // @User: Advanced + // @Values: 0:None,1:Roll,2:Pitch,4:Yaw,8:AccelZ + // @Bitmask: 0:Roll,1:Pitch,2:Yaw,3:AccelZ + GSCALAR(gcs_pid_mask, "GCS_PID_MASK", 0), + + // #if MODE_RTL_ENABLED == ENABLED + // // @Param: RTL_ALT + // // @DisplayName: RTL Altitude + // // @Description: The minimum alt above home the vehicle will climb to before returning. If the vehicle is flying higher than this value it will return at its current altitude. + // // @Units: cm + // // @Range: 200 8000 + // // @Increment: 1 + // // @User: Standard + // GSCALAR(rtl_altitude, "RTL_ALT", RTL_ALT), + + // // @Param: RTL_CONE_SLOPE + // // @DisplayName: RTL cone slope + // // @Description: Defines a cone above home which determines maximum climb + // // @Range: 0.5 10.0 + // // @Increment: .1 + // // @Values: 0:Disabled,1:Shallow,3:Steep + // // @User: Standard + // GSCALAR(rtl_cone_slope, "RTL_CONE_SLOPE", RTL_CONE_SLOPE_DEFAULT), + + // // @Param: RTL_SPEED + // // @DisplayName: RTL speed + // // @Description: Defines the speed in cm/s which the aircraft will attempt to maintain horizontally while flying home. If this is set to zero, WPNAV_SPEED will be used instead. + // // @Units: cm/s + // // @Range: 0 2000 + // // @Increment: 50 + // // @User: Standard + // GSCALAR(rtl_speed_cms, "RTL_SPEED", 0), + + // // @Param: RTL_ALT_FINAL + // // @DisplayName: RTL Final Altitude + // // @Description: This is the altitude the vehicle will move to as the final stage of Returning to Launch or after completing a mission. Set to zero to land. + // // @Units: cm + // // @Range: 0 1000 + // // @Increment: 1 + // // @User: Standard + // GSCALAR(rtl_alt_final, "RTL_ALT_FINAL", RTL_ALT_FINAL), + + // // @Param: RTL_CLIMB_MIN + // // @DisplayName: RTL minimum climb + // // @Description: The vehicle will climb this many cm during the initial climb portion of the RTL + // // @Units: cm + // // @Range: 0 3000 + // // @Increment: 10 + // // @User: Standard + // GSCALAR(rtl_climb_min, "RTL_CLIMB_MIN", RTL_CLIMB_MIN_DEFAULT), + + // // @Param: RTL_LOIT_TIME + // // @DisplayName: RTL loiter time + // // @Description: Time (in milliseconds) to loiter above home before beginning final descent + // // @Units: ms + // // @Range: 0 60000 + // // @Increment: 1000 + // // @User: Standard + // GSCALAR(rtl_loiter_time, "RTL_LOIT_TIME", RTL_LOITER_TIME), + // #endif + + // @Param: FS_GCS_ENABLE + // @DisplayName: Ground Station Failsafe Enable + // @Description: Controls whether failsafe will be invoked (and what action to take) when connection with Ground station is lost for at least 5 seconds. See FS_OPTIONS param for additional actions, or for cases allowing Mission continuation, when GCS failsafe is enabled. + // @Values: 0:Disabled/NoAction,1:RTL,2:RTL or Continue with Mission in Auto Mode (Removed in 4.0+-see FS_OPTIONS),3:SmartRTL or RTL,4:SmartRTL or Land,5:Land (4.0+ Only) + // @User: Standard + GSCALAR(failsafe_gcs, "FS_GCS_ENABLE", FS_GCS_DISABLED), + + // @Param: GPS_HDOP_GOOD + // @DisplayName: GPS Hdop Good + // @Description: GPS Hdop value at or below this value represent a good position. Used for pre-arm checks + // @Range: 100 900 + // @User: Advanced + GSCALAR(gps_hdop_good, "GPS_HDOP_GOOD", GPS_HDOP_GOOD_DEFAULT), + + // @Param: WP_YAW_BEHAVIOR + // @DisplayName: Yaw behaviour during missions + // @Description: Determines how the autopilot controls the yaw during missions and RTL + // @Values: 0:Never change yaw, 1:Face next waypoint, 2:Face next waypoint except RTL, 3:Face along GPS course + // @User: Standard + GSCALAR(wp_yaw_behavior, "WP_YAW_BEHAVIOR", WP_YAW_BEHAVIOR_DEFAULT), + + // @Param: LAND_SPEED + // @DisplayName: Land speed + // @Description: The descent speed for the final stage of landing in cm/s + // @Units: cm/s + // @Range: 30 200 + // @Increment: 10 + // @User: Standard + GSCALAR(land_speed, "LAND_SPEED", LAND_SPEED), + + // @Param: LAND_SPEED_HIGH + // @DisplayName: Land speed high + // @Description: The descent speed for the first stage of landing in cm/s. If this is zero then WPNAV_SPEED_DN is used + // @Units: cm/s + // @Range: 0 500 + // @Increment: 10 + // @User: Standard + GSCALAR(land_speed_high, "LAND_SPEED_HIGH", 0), + + // @Param: PILOT_SPEED_UP + // @DisplayName: Pilot maximum vertical speed ascending + // @Description: The maximum vertical ascending velocity the pilot may request in cm/s + // @Units: cm/s + // @Range: 50 500 + // @Increment: 10 + // @User: Standard + GSCALAR(pilot_speed_up, "PILOT_SPEED_UP", PILOT_VELZ_MAX), + + // @Param: PILOT_ACCEL_Z + // @DisplayName: Pilot vertical acceleration + // @Description: The vertical acceleration used when pilot is controlling the altitude + // @Units: cm/s/s + // @Range: 50 500 + // @Increment: 10 + // @User: Standard + GSCALAR(pilot_accel_z, "PILOT_ACCEL_Z", PILOT_ACCEL_Z_DEFAULT), + + // @Param: FS_THR_ENABLE + // @DisplayName: Throttle Failsafe Enable + // @Description: The throttle failsafe allows you to configure a software failsafe activated by a setting on the throttle input channel + // @Values: 0:Disabled,1:Enabled always RTL,2:Enabled Continue with Mission in Auto Mode (Removed in 4.0+),3:Enabled always Land,4:Enabled always SmartRTL or RTL,5:Enabled always SmartRTL or Land + // @User: Standard + GSCALAR(failsafe_throttle, "FS_THR_ENABLE", FS_THR_ENABLED_ALWAYS_RTL), + + // @Param: FS_THR_VALUE + // @DisplayName: Throttle Failsafe Value + // @Description: The PWM level in microseconds on channel 3 below which throttle failsafe triggers + // @Range: 910 1100 + // @Units: PWM + // @Increment: 1 + // @User: Standard + GSCALAR(failsafe_throttle_value, "FS_THR_VALUE", FS_THR_VALUE_DEFAULT), + + // @Param: THR_DZ + // @DisplayName: Throttle deadzone + // @Description: The deadzone above and below mid throttle in PWM microseconds. Used in AltHold, Loiter, PosHold flight modes + // @User: Standard + // @Range: 0 300 + // @Units: PWM + // @Increment: 1 + GSCALAR(throttle_deadzone, "THR_DZ", THR_DZ_DEFAULT), + + // @Param: FLTMODE1 + // @DisplayName: Flight Mode 1 + // @Description: Flight mode when Channel 5 pwm is <= 1230 + // @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,9:Land,11:Drift,13:Sport,14:Flip,15:AutoTune,16:PosHold,17:Brake,18:Throw,19:Avoid_ADSB,20:Guided_NoGPS,21:Smart_RTL,22:FlowHold,23:Follow,24:ZigZag,25:SystemID,26:Heli_Autorotate + // @User: Standard + GSCALAR(flight_mode1, "FLTMODE1", (uint8_t)FLIGHT_MODE_1), + + // @Param: FLTMODE2 + // @DisplayName: Flight Mode 2 + // @Description: Flight mode when Channel 5 pwm is >1230, <= 1360 + // @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,9:Land,11:Drift,13:Sport,14:Flip,15:AutoTune,16:PosHold,17:Brake,18:Throw,19:Avoid_ADSB,20:Guided_NoGPS,21:Smart_RTL,22:FlowHold,23:Follow,24:ZigZag,25:SystemID,26:Heli_Autorotate + // @User: Standard + GSCALAR(flight_mode2, "FLTMODE2", (uint8_t)FLIGHT_MODE_2), + + // @Param: FLTMODE3 + // @DisplayName: Flight Mode 3 + // @Description: Flight mode when Channel 5 pwm is >1360, <= 1490 + // @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,9:Land,11:Drift,13:Sport,14:Flip,15:AutoTune,16:PosHold,17:Brake,18:Throw,19:Avoid_ADSB,20:Guided_NoGPS,21:Smart_RTL,22:FlowHold,23:Follow,24:ZigZag,25:SystemID,26:Heli_Autorotate + // @User: Standard + GSCALAR(flight_mode3, "FLTMODE3", (uint8_t)FLIGHT_MODE_3), + + // @Param: FLTMODE4 + // @DisplayName: Flight Mode 4 + // @Description: Flight mode when Channel 5 pwm is >1490, <= 1620 + // @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,9:Land,11:Drift,13:Sport,14:Flip,15:AutoTune,16:PosHold,17:Brake,18:Throw,19:Avoid_ADSB,20:Guided_NoGPS,21:Smart_RTL,22:FlowHold,23:Follow,24:ZigZag,25:SystemID,26:Heli_Autorotate + // @User: Standard + GSCALAR(flight_mode4, "FLTMODE4", (uint8_t)FLIGHT_MODE_4), + + // @Param: FLTMODE5 + // @DisplayName: Flight Mode 5 + // @Description: Flight mode when Channel 5 pwm is >1620, <= 1749 + // @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,9:Land,11:Drift,13:Sport,14:Flip,15:AutoTune,16:PosHold,17:Brake,18:Throw,19:Avoid_ADSB,20:Guided_NoGPS,21:Smart_RTL,22:FlowHold,23:Follow,24:ZigZag,25:SystemID,26:Heli_Autorotate + // @User: Standard + GSCALAR(flight_mode5, "FLTMODE5", (uint8_t)FLIGHT_MODE_5), + + // @Param: FLTMODE6 + // @DisplayName: Flight Mode 6 + // @Description: Flight mode when Channel 5 pwm is >=1750 + // @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,9:Land,11:Drift,13:Sport,14:Flip,15:AutoTune,16:PosHold,17:Brake,18:Throw,19:Avoid_ADSB,20:Guided_NoGPS,21:Smart_RTL,22:FlowHold,23:Follow,24:ZigZag,25:SystemID,26:Heli_Autorotate + // @User: Standard + GSCALAR(flight_mode6, "FLTMODE6", (uint8_t)FLIGHT_MODE_6), + + // @Param: FLTMODE_CH + // @DisplayName: Flightmode channel + // @Description: RC Channel to use for flight mode control + // @Values: 0:Disabled,5:Channel5,6:Channel6,7:Channel7,8:Channel8 + // @User: Advanced + GSCALAR(flight_mode_chan, "FLTMODE_CH", CH_MODE_DEFAULT), + + // @Param: INITIAL_MODE + // @DisplayName: Initial flight mode + // @Description: This selects the mode to start in on boot. This is useful for when you want to start in AUTO mode on boot without a receiver. + // @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,9:Land,11:Drift,13:Sport,14:Flip,15:AutoTune,16:PosHold,17:Brake,18:Throw,19:Avoid_ADSB,20:Guided_NoGPS,21:Smart_RTL,22:FlowHold,23:Follow,24:ZigZag,25:SystemID,26:Heli_Autorotate + // @User: Advanced + GSCALAR(initial_mode, "INITIAL_MODE", (uint8_t)Mode::Number::MANUAL), + + // @Param: LOG_BITMASK + // @DisplayName: Log bitmask + // @Description: 4 byte bitmap of log types to enable + // @Values: 830:Default,894:Default+RCIN,958:Default+IMU,1854:Default+Motors,-6146:NearlyAll-AC315,45054:NearlyAll,131071:All+FastATT,262142:All+MotBatt,393214:All+FastIMU,397310:All+FastIMU+PID,655358:All+FullIMU,0:Disabled + // @Bitmask: 0:ATTITUDE_FAST,1:ATTITUDE_MED,2:GPS,3:PM,4:CTUN,5:NTUN,6:RCIN,7:IMU,8:CMD,9:CURRENT,10:RCOUT,11:OPTFLOW,12:PID,13:COMPASS,14:INAV,15:CAMERA,17:MOTBATT,18:IMU_FAST,19:IMU_RAW + // @User: Standard + GSCALAR(log_bitmask, "LOG_BITMASK", DEFAULT_LOG_BITMASK), + + // @Param: TUNE + // @DisplayName: Channel 6 Tuning + // @Description: Controls which parameters (normally PID gains) are being tuned with transmitter's channel 6 knob + // @User: Standard + // @Values: 0:None,1:Stab Roll/Pitch kP,4:Rate Roll/Pitch kP,5:Rate Roll/Pitch kI,21:Rate Roll/Pitch kD,3:Stab Yaw kP,6:Rate Yaw kP,26:Rate Yaw kD,56:Rate Yaw Filter,55:Motor Yaw Headroom,14:AltHold kP,7:Throttle Rate kP,34:Throttle Accel kP,35:Throttle Accel kI,36:Throttle Accel kD,12:Loiter Pos kP,22:Velocity XY kP,28:Velocity XY kI,10:WP Speed,25:Acro RollPitch kP,40:Acro Yaw kP,45:RC Feel,13:Heli Ext Gyro,38:Declination,39:Circle Rate,41:RangeFinder Gain,46:Rate Pitch kP,47:Rate Pitch kI,48:Rate Pitch kD,49:Rate Roll kP,50:Rate Roll kI,51:Rate Roll kD,52:Rate Pitch FF,53:Rate Roll FF,54:Rate Yaw FF,58:SysID Magnitude + GSCALAR(radio_tuning, "TUNE", 0), + + // @Param: FRAME_TYPE + // @DisplayName: Frame Type (+, X, V, etc) + // @Description: Controls motor mixing for multiblimps. Not used for Tri or Traditional Heliblimps. + // @Values: 0:Plus, 1:X, 2:V, 3:H, 4:V-Tail, 5:A-Tail, 10:Y6B, 11:Y6F, 12:BetaFlightX, 13:DJIX, 14:ClockwiseX, 15: I, 18: BetaFlightXReversed + // @User: Standard + // @RebootRequired: True + GSCALAR(frame_type, "FRAME_TYPE", HAL_FRAME_TYPE_DEFAULT), + + // @Group: ARMING_ + // @Path: ../libraries/AP_Arming/AP_Arming.cpp + GOBJECT(arming, "ARMING_", AP_Arming_Blimp), + + // @Param: DISARM_DELAY + // @DisplayName: Disarm delay + // @Description: Delay before automatic disarm in seconds. A value of zero disables auto disarm. + // @Units: s + // @Range: 0 127 + // @User: Advanced + GSCALAR(disarm_delay, "DISARM_DELAY", AUTO_DISARMING_DELAY), + + // @Param: ANGLE_MAX + // @DisplayName: Angle Max + // @Description: Maximum lean angle in all flight modes + // @Units: cdeg + // @Range: 1000 8000 + // @User: Advanced + ASCALAR(angle_max, "ANGLE_MAX", DEFAULT_ANGLE_MAX), + + // @Param: PHLD_BRAKE_RATE + // @DisplayName: PosHold braking rate + // @Description: PosHold flight mode's rotation rate during braking in deg/sec + // @Units: deg/s + // @Range: 4 12 + // @User: Advanced + GSCALAR(poshold_brake_rate, "PHLD_BRAKE_RATE", POSHOLD_BRAKE_RATE_DEFAULT), + + // @Param: PHLD_BRAKE_ANGLE + // @DisplayName: PosHold braking angle max + // @Description: PosHold flight mode's max lean angle during braking in centi-degrees + // @Units: cdeg + // @Range: 2000 4500 + // @User: Advanced + GSCALAR(poshold_brake_angle_max, "PHLD_BRAKE_ANGLE", POSHOLD_BRAKE_ANGLE_DEFAULT), + + // @Param: LAND_REPOSITION + // @DisplayName: Land repositioning + // @Description: Enables user input during LAND mode, the landing phase of RTL, and auto mode landings. + // @Values: 0:No repositioning, 1:Repositioning + // @User: Advanced + GSCALAR(land_repositioning, "LAND_REPOSITION", LAND_REPOSITION_DEFAULT), + + // @Param: FS_EKF_ACTION + // @DisplayName: EKF Failsafe Action + // @Description: Controls the action that will be taken when an EKF failsafe is invoked + // @Values: 1:Land, 2:AltHold, 3:Land even in Stabilize + // @User: Advanced + GSCALAR(fs_ekf_action, "FS_EKF_ACTION", FS_EKF_ACTION_DEFAULT), + + // @Param: FS_EKF_THRESH + // @DisplayName: EKF failsafe variance threshold + // @Description: Allows setting the maximum acceptable compass and velocity variance + // @Values: 0.6:Strict, 0.8:Default, 1.0:Relaxed + // @User: Advanced + GSCALAR(fs_ekf_thresh, "FS_EKF_THRESH", FS_EKF_THRESHOLD_DEFAULT), + + // @Param: FS_CRASH_CHECK + // @DisplayName: Crash check enable + // @Description: This enables automatic crash checking. When enabled the motors will disarm if a crash is detected. + // @Values: 0:Disabled, 1:Enabled + // @User: Advanced + GSCALAR(fs_crash_check, "FS_CRASH_CHECK", 1), + + // @Param: RC_SPEED + // @DisplayName: ESC Update Speed + // @Description: This is the speed in Hertz that your ESCs will receive updates + // @Units: Hz + // @Range: 50 490 + // @Increment: 1 + // @User: Advanced + GSCALAR(rc_speed, "RC_SPEED", RC_FAST_SPEED), + + // @Param: ACRO_RP_P + // @DisplayName: Acro Roll and Pitch P gain + // @Description: Converts pilot roll and pitch into a desired rate of rotation in ACRO and SPORT mode. Higher values mean faster rate of rotation. + // @Range: 1 10 + // @User: Standard + GSCALAR(acro_rp_p, "ACRO_RP_P", ACRO_RP_P), + + // @Param: ACRO_YAW_P + // @DisplayName: Acro Yaw P gain + // @Description: Converts pilot yaw input into a desired rate of rotation. Higher values mean faster rate of rotation. + // @Range: 1 10 + // @User: Standard + GSCALAR(acro_yaw_p, "ACRO_YAW_P", ACRO_YAW_P), + + // variables not in the g class which contain EEPROM saved variables + + // @Group: RELAY_ + // @Path: ../libraries/AP_Relay/AP_Relay.cpp + GOBJECT(relay, "RELAY_", AP_Relay), + + // @Group: COMPASS_ + // @Path: ../libraries/AP_Compass/AP_Compass.cpp + GOBJECT(compass, "COMPASS_", Compass), + + // @Group: INS_ + // @Path: ../libraries/AP_InertialSensor/AP_InertialSensor.cpp + GOBJECT(ins, "INS_", AP_InertialSensor), + + // // @Group: WPNAV_ + // // @Path: ../libraries/AC_WPNav/AC_WPNav.cpp + // GOBJECTPTR(wp_nav, "WPNAV_", AC_WPNav), + + // // @Group: LOIT_ + // // @Path: ../libraries/AC_WPNav/AC_Loiter.cpp + // GOBJECTPTR(loiter_nav, "LOIT_", AC_Loiter), + + // @Group: ATC_ + // @Path: ../libraries/AC_AttitudeControl/AC_AttitudeControl.cpp,../libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.cpp,../libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp + // #if FRAME_CONFIG == HELI_FRAME + // GOBJECTPTR(attitude_control, "ATC_", AC_AttitudeControl_Heli), + // #else + // GOBJECTPTR(attitude_control, "ATC_", AC_AttitudeControl_Multi), + // #endif + + // @Group: PSC + // @Path: ../libraries/AC_AttitudeControl/AC_PosControl.cpp + // GOBJECTPTR(pos_control, "PSC", AC_PosControl), + + // @Group: SR0_ + // @Path: GCS_Mavlink.cpp + GOBJECTN(_gcs.chan_parameters[0], gcs0, "SR0_", GCS_MAVLINK_Parameters), + +#if MAVLINK_COMM_NUM_BUFFERS >= 2 + // @Group: SR1_ + // @Path: GCS_Mavlink.cpp + GOBJECTN(_gcs.chan_parameters[1], gcs1, "SR1_", GCS_MAVLINK_Parameters), +#endif + +#if MAVLINK_COMM_NUM_BUFFERS >= 3 + // @Group: SR2_ + // @Path: GCS_Mavlink.cpp + GOBJECTN(_gcs.chan_parameters[2], gcs2, "SR2_", GCS_MAVLINK_Parameters), +#endif + +#if MAVLINK_COMM_NUM_BUFFERS >= 4 + // @Group: SR3_ + // @Path: GCS_Mavlink.cpp + GOBJECTN(_gcs.chan_parameters[3], gcs3, "SR3_", GCS_MAVLINK_Parameters), +#endif + +#if MAVLINK_COMM_NUM_BUFFERS >= 5 + // @Group: SR4_ + // @Path: GCS_Mavlink.cpp + GOBJECTN(_gcs.chan_parameters[4], gcs4, "SR4_", GCS_MAVLINK_Parameters), +#endif + +#if MAVLINK_COMM_NUM_BUFFERS >= 6 + // @Group: SR5_ + // @Path: GCS_Mavlink.cpp + GOBJECTN(_gcs.chan_parameters[5], gcs5, "SR5_", GCS_MAVLINK_Parameters), +#endif + +#if MAVLINK_COMM_NUM_BUFFERS >= 7 + // @Group: SR6_ + // @Path: GCS_Mavlink.cpp + GOBJECTN(_gcs.chan_parameters[6], gcs6, "SR6_", GCS_MAVLINK_Parameters), +#endif + + // @Group: AHRS_ + // @Path: ../libraries/AP_AHRS/AP_AHRS.cpp + GOBJECT(ahrs, "AHRS_", AP_AHRS), + + // @Group: LOG + // @Path: ../libraries/AP_Logger/AP_Logger.cpp + GOBJECT(logger, "LOG", AP_Logger), + + // @Group: BATT + // @Path: ../libraries/AP_BattMonitor/AP_BattMonitor.cpp + GOBJECT(battery, "BATT", AP_BattMonitor), + + // @Group: BRD_ + // @Path: ../libraries/AP_BoardConfig/AP_BoardConfig.cpp + GOBJECT(BoardConfig, "BRD_", AP_BoardConfig), + +#if HAL_MAX_CAN_PROTOCOL_DRIVERS + // @Group: CAN_ + // @Path: ../libraries/AP_CANManager/AP_CANManager.cpp + GOBJECT(can_mgr, "CAN_", AP_CANManager), +#endif + +#if CONFIG_HAL_BOARD == HAL_BOARD_SITL + GOBJECT(sitl, "SIM_", SITL::SITL), +#endif + + // @Group: BARO + // @Path: ../libraries/AP_Baro/AP_Baro.cpp + GOBJECT(barometer, "BARO", AP_Baro), + + // GPS driver + // @Group: GPS_ + // @Path: ../libraries/AP_GPS/AP_GPS.cpp + GOBJECT(gps, "GPS_", AP_GPS), + + // @Group: SCHED_ + // @Path: ../libraries/AP_Scheduler/AP_Scheduler.cpp + GOBJECT(scheduler, "SCHED_", AP_Scheduler), + + // @Group: RCMAP_ + // @Path: ../libraries/AP_RCMapper/AP_RCMapper.cpp + GOBJECT(rcmap, "RCMAP_", RCMapper), + +#if HAL_NAVEKF2_AVAILABLE + // @Group: EK2_ + // @Path: ../libraries/AP_NavEKF2/AP_NavEKF2.cpp + GOBJECTN(ahrs.EKF2, NavEKF2, "EK2_", NavEKF2), +#endif + +#if HAL_NAVEKF3_AVAILABLE + // @Group: EK3_ + // @Path: ../libraries/AP_NavEKF3/AP_NavEKF3.cpp + GOBJECTN(ahrs.EKF3, NavEKF3, "EK3_", NavEKF3), +#endif + + // @Group: RSSI_ + // @Path: ../libraries/AP_RSSI/AP_RSSI.cpp + GOBJECT(rssi, "RSSI_", AP_RSSI), + + // @Group: NTF_ + // @Path: ../libraries/AP_Notify/AP_Notify.cpp + GOBJECT(notify, "NTF_", AP_Notify), + + // @Group: + // @Path: Parameters.cpp + GOBJECT(g2, "", ParametersG2), + + // @Group: FINS_ + // @Path: Fins.cpp + GOBJECTPTR(motors, "FINS_", Fins), + + // @Group: + // @Path: ../libraries/AP_Vehicle/AP_Vehicle.cpp + { AP_PARAM_GROUP, "", Parameters::k_param_vehicle, (const void *)&blimp, {group_info : AP_Vehicle::var_info} }, + + AP_VAREND +}; + +/* + 2nd group of parameters + */ +const AP_Param::GroupInfo ParametersG2::var_info[] = { + + // @Param: WP_NAVALT_MIN + // @DisplayName: Minimum navigation altitude + // @Description: This is the altitude in meters above which for navigation can begin. This applies in auto takeoff and auto landing. + // @Range: 0 5 + // @User: Standard + AP_GROUPINFO("WP_NAVALT_MIN", 1, ParametersG2, wp_navalt_min, 0), + + // @Param: DEV_OPTIONS + // @DisplayName: Development options + // @Description: Bitmask of developer options. The meanings of the bit fields in this parameter may vary at any time. Developers should check the source code for current meaning + // @Bitmask: 0:ADSBMavlinkProcessing,1:DevOptionVFR_HUDRelativeAlt,2:SetAttitudeTarget_ThrustAsThrust + // @User: Advanced + AP_GROUPINFO("DEV_OPTIONS", 7, ParametersG2, dev_options, 0), + + // @Param: ACRO_Y_EXPO + // @DisplayName: Acro Yaw Expo + // @Description: Acro yaw expo to allow faster rotation when stick at edges + // @Values: 0:Disabled,0.1:Very Low,0.2:Low,0.3:Medium,0.4:High,0.5:Very High + // @Range: -0.5 1.0 + // @User: Advanced + AP_GROUPINFO("ACRO_Y_EXPO", 9, ParametersG2, acro_y_expo, ACRO_Y_EXPO_DEFAULT), + + + // @Param: SYSID_ENFORCE + // @DisplayName: GCS sysid enforcement + // @Description: This controls whether packets from other than the expected GCS system ID will be accepted + // @Values: 0:NotEnforced,1:Enforced + // @User: Advanced + AP_GROUPINFO("SYSID_ENFORCE", 11, ParametersG2, sysid_enforce, 0), + +#if STATS_ENABLED == ENABLED + // @Group: STAT + // @Path: ../libraries/AP_Stats/AP_Stats.cpp + AP_SUBGROUPINFO(stats, "STAT", 12, ParametersG2, AP_Stats), +#endif + + // @Param: FRAME_CLASS + // @DisplayName: Frame Class + // @Description: Controls major frame class for multiblimp component + // @Values: 0:Undefined, 1:Quad, 2:Hexa, 3:Octa, 4:OctaQuad, 5:Y6, 6:Heli, 7:Tri, 8:SingleCopter, 9:CoaxCopter, 10:BiCopter, 11:Heli_Dual, 12:DodecaHexa, 13:HeliQuad, 14:Deca + // @User: Standard + // @RebootRequired: True + AP_GROUPINFO("FRAME_CLASS", 15, ParametersG2, frame_class, DEFAULT_FRAME_CLASS), + + // @Group: SERVO + // @Path: ../libraries/SRV_Channel/SRV_Channels.cpp + AP_SUBGROUPINFO(servo_channels, "SERVO", 16, ParametersG2, SRV_Channels), + + // @Group: RC + // @Path: ../libraries/RC_Channel/RC_Channels_VarInfo.h + AP_SUBGROUPINFO(rc_channels, "RC", 17, ParametersG2, RC_Channels_Blimp), + + // // 18 was used by AP_VisualOdom + + // // @Group: TCAL + // // @Path: ../libraries/AP_TempCalibration/AP_TempCalibration.cpp + // AP_SUBGROUPINFO(temp_calibration, "TCAL", 19, ParametersG2, AP_TempCalibration), + + // @Param: PILOT_SPEED_DN + // @DisplayName: Pilot maximum vertical speed descending + // @Description: The maximum vertical descending velocity the pilot may request in cm/s + // @Units: cm/s + // @Range: 50 500 + // @Increment: 10 + // @User: Standard + AP_GROUPINFO("PILOT_SPEED_DN", 24, ParametersG2, pilot_speed_dn, 0), + + // @Param: LAND_ALT_LOW + // @DisplayName: Land alt low + // @Description: Altitude during Landing at which vehicle slows to LAND_SPEED + // @Units: cm + // @Range: 100 10000 + // @Increment: 10 + // @User: Advanced + AP_GROUPINFO("LAND_ALT_LOW", 25, ParametersG2, land_alt_low, 1000), + +#ifdef ENABLE_SCRIPTING + // @Group: SCR_ + // @Path: ../libraries/AP_Scripting/AP_Scripting.cpp + AP_SUBGROUPINFO(scripting, "SCR_", 30, ParametersG2, AP_Scripting), +#endif + + // @Param: TUNE_MIN + // @DisplayName: Tuning minimum + // @Description: Minimum value that the parameter currently being tuned with the transmitter's channel 6 knob will be set to + // @User: Standard + AP_GROUPINFO("TUNE_MIN", 31, ParametersG2, tuning_min, 0), + + // @Param: TUNE_MAX + // @DisplayName: Tuning maximum + // @Description: Maximum value that the parameter currently being tuned with the transmitter's channel 6 knob will be set to + // @User: Standard + AP_GROUPINFO("TUNE_MAX", 32, ParametersG2, tuning_max, 0), + + // @Param: FS_VIBE_ENABLE + // @DisplayName: Vibration Failsafe enable + // @Description: This enables the vibration failsafe which will use modified altitude estimation and control during high vibrations + // @Values: 0:Disabled, 1:Enabled + // @User: Standard + AP_GROUPINFO("FS_VIBE_ENABLE", 35, ParametersG2, fs_vibe_enabled, 1), + + // @Param: FS_OPTIONS + // @DisplayName: Failsafe options bitmask + // @Description: Bitmask of additional options for battery, radio, & GCS failsafes. 0 (default) disables all options. + // @Values: 0:Disabled, 1:Continue if in Auto on RC failsafe only, 2:Continue if in Auto on GCS failsafe only, 3:Continue if in Auto on RC and/or GCS failsafe, 4:Continue if in Guided on RC failsafe only, 8:Continue if landing on any failsafe, 16:Continue if in pilot controlled modes on GCS failsafe, 19:Continue if in Auto on RC and/or GCS failsafe and continue if in pilot controlled modes on GCS failsafe + // @Bitmask: 0:Continue if in Auto on RC failsafe, 1:Continue if in Auto on GCS failsafe, 2:Continue if in Guided on RC failsafe, 3:Continue if landing on any failsafe, 4:Continue if in pilot controlled modes on GCS failsafe, 5:Release Gripper + // @User: Advanced + AP_GROUPINFO("FS_OPTIONS", 36, ParametersG2, fs_options, (float)Blimp::FailsafeOption::GCS_CONTINUE_IF_PILOT_CONTROL), + + // @Param: FS_GCS_TIMEOUT + // @DisplayName: GCS failsafe timeout + // @Description: Timeout before triggering the GCS failsafe + // @Units: s + // @Range: 2 120 + // @Increment: 1 + // @User: Standard + AP_GROUPINFO("FS_GCS_TIMEOUT", 42, ParametersG2, fs_gcs_timeout, 5), + + AP_GROUPEND +}; + +/* + constructor for g2 object + */ +ParametersG2::ParametersG2(void) +// : temp_calibration() // this doesn't actually need constructing, but removing it here is problematic syntax-wise +{ + AP_Param::setup_object_defaults(this, var_info); +} + + +void Blimp::load_parameters(void) +{ + if (!AP_Param::check_var_info()) { + hal.console->printf("Bad var table\n"); + AP_HAL::panic("Bad var table"); + } + + // disable centrifugal force correction, it will be enabled as part of the arming process + ahrs.set_correct_centrifugal(false); + hal.util->set_soft_armed(false); + + if (!g.format_version.load() || + g.format_version != Parameters::k_format_version) { + + // erase all parameters + hal.console->printf("Firmware change: erasing EEPROM...\n"); + StorageManager::erase(); + AP_Param::erase_all(); + + // save the current format version + g.format_version.set_and_save(Parameters::k_format_version); + hal.console->printf("done.\n"); + } + + uint32_t before = micros(); + // Load all auto-loaded EEPROM variables + AP_Param::load_all(); + + hal.console->printf("load_all took %uus\n", (unsigned)(micros() - before)); + + // setup AP_Param frame type flags + AP_Param::set_frame_type_flags(AP_PARAM_FRAME_COPTER); + +} diff --git a/Blimp/Parameters.h b/Blimp/Parameters.h new file mode 100644 index 0000000000..f916c3374d --- /dev/null +++ b/Blimp/Parameters.h @@ -0,0 +1,529 @@ +#pragma once + +#include +#include "RC_Channel.h" + +// Global parameter class. +// +class Parameters +{ +public: + // The version of the layout as described by the parameter enum. + // + // When changing the parameter enum in an incompatible fashion, this + // value should be incremented by one. + // + // The increment will prevent old parameters from being used incorrectly + // by newer code. + // + static const uint16_t k_format_version = 120; + + // Parameter identities. + // + // The enumeration defined here is used to ensure that every parameter + // or parameter group has a unique ID number. This number is used by + // AP_Param to store and locate parameters in EEPROM. + // + // Note that entries without a number are assigned the next number after + // the entry preceding them. When adding new entries, ensure that they + // don't overlap. + // + // Try to group related variables together, and assign them a set + // range in the enumeration. Place these groups in numerical order + // at the end of the enumeration. + // + // WARNING: Care should be taken when editing this enumeration as the + // AP_Param load/save code depends on the values here to identify + // variables saved in EEPROM. + // + // + enum { + // Layout version number, always key zero. + // + k_param_format_version = 0, + k_param_software_type, // deprecated + k_param_ins_old, // *** Deprecated, remove with next eeprom number change + k_param_ins, // libraries/AP_InertialSensor variables + k_param_NavEKF2_old, // deprecated - remove + k_param_NavEKF2, + k_param_g2, // 2nd block of parameters + k_param_NavEKF3, + k_param_can_mgr, + k_param_osd, + + // simulation + k_param_sitl = 10, + + // barometer object (needed for SITL) + k_param_barometer, + + // scheduler object (for debugging) + k_param_scheduler, + + // relay object + k_param_relay, + + // (old) EPM object + k_param_epm_unused, + + // BoardConfig object + k_param_BoardConfig, + + // GPS object + k_param_gps, + + // Parachute object + k_param_parachute, + + // Landing gear object + k_param_landinggear, // 18 + + // Input Management object + k_param_input_manager, // 19 + + // Misc + // + k_param_log_bitmask_old = 20, // Deprecated + k_param_log_last_filenumber, // *** Deprecated - remove + // with next eeprom number + // change + k_param_toy_yaw_rate, // deprecated - remove + k_param_crosstrack_min_distance, // deprecated - remove with next eeprom number change + k_param_rssi_pin, // unused, replaced by rssi_ library parameters + k_param_throttle_accel_enabled, // deprecated - remove + k_param_wp_yaw_behavior, + k_param_acro_trainer, + k_param_pilot_speed_up, // renamed from k_param_pilot_velocity_z_max + k_param_circle_rate, // deprecated - remove + k_param_rangefinder_gain, + k_param_ch8_option_old, // deprecated + k_param_arming_check_old, // deprecated - remove + k_param_sprayer, + k_param_angle_max, + k_param_gps_hdop_good, + k_param_battery, + k_param_fs_batt_mah, // unused - moved to AP_BattMonitor + k_param_angle_rate_max, // remove + k_param_rssi_range, // unused, replaced by rssi_ library parameters + k_param_rc_feel_rp, // deprecated + k_param_NavEKF, // deprecated - remove + k_param_mission, // mission library + k_param_rc_13_old, + k_param_rc_14_old, + k_param_rally, + k_param_poshold_brake_rate, + k_param_poshold_brake_angle_max, + k_param_pilot_accel_z, + k_param_serial0_baud, // deprecated - remove + k_param_serial1_baud, // deprecated - remove + k_param_serial2_baud, // deprecated - remove + k_param_land_repositioning, + k_param_rangefinder, // rangefinder object + k_param_fs_ekf_thresh, + k_param_terrain, + k_param_acro_rp_expo, + k_param_throttle_deadzone, + k_param_optflow, + k_param_dcmcheck_thresh, // deprecated - remove + k_param_log_bitmask, + k_param_cli_enabled_old, // deprecated - remove + k_param_throttle_filt, + k_param_throttle_behavior, + k_param_pilot_takeoff_alt, // 64 + + // 65: AP_Limits Library + k_param_limits = 65, // deprecated - remove + k_param_gpslock_limit, // deprecated - remove + k_param_geofence_limit, // deprecated - remove + k_param_altitude_limit, // deprecated - remove + k_param_fence, + k_param_gps_glitch, // deprecated + k_param_baro_glitch, // 71 - deprecated + + // AP_ADSB Library + k_param_adsb, // 72 + k_param_notify, // 73 + + // 74: precision landing object + k_param_precland = 74, + + // + // 75: Singlecopter, CoaxBlimp + // + k_param_single_servo_1 = 75, // remove + k_param_single_servo_2, // remove + k_param_single_servo_3, // remove + k_param_single_servo_4, // 78 - remove + + // + // 80: Heli + // + k_param_heli_servo_1 = 80, // remove + k_param_heli_servo_2, // remove + k_param_heli_servo_3, // remove + k_param_heli_servo_4, // remove + k_param_heli_pitch_ff, // remove + k_param_heli_roll_ff, // remove + k_param_heli_yaw_ff, // remove + k_param_heli_stab_col_min, // remove + k_param_heli_stab_col_max, // remove + k_param_heli_servo_rsc, // 89 = full! - remove + + // + // 90: misc2 + // + k_param_motors = 90, + k_param_disarm_delay, + k_param_fs_crash_check, + k_param_throw_motor_start, + k_param_rtl_alt_type, + k_param_avoid, + k_param_avoidance_adsb, + + // 97: RSSI + k_param_rssi = 97, + + // + // 100: Inertial Nav + // + k_param_inertial_nav = 100, // deprecated + k_param_wp_nav, + k_param_attitude_control, + k_param_pos_control, + k_param_circle_nav, + k_param_loiter_nav, // 105 + + // 110: Telemetry control + // + k_param_gcs0 = 110, + k_param_gcs1, + k_param_sysid_this_mav, + k_param_sysid_my_gcs, + k_param_serial1_baud_old, // deprecated + k_param_telem_delay, + k_param_gcs2, + k_param_serial2_baud_old, // deprecated + k_param_serial2_protocol, // deprecated + k_param_serial_manager, + k_param_ch9_option_old, + k_param_ch10_option_old, + k_param_ch11_option_old, + k_param_ch12_option_old, + k_param_takeoff_trigger_dz_old, + k_param_gcs3, + k_param_gcs_pid_mask, // 126 + k_param_gcs4, + k_param_gcs5, + k_param_gcs6, + + // + // 135 : reserved for Solo until features merged with master + // + k_param_rtl_speed_cms = 135, + k_param_fs_batt_curr_rtl, + k_param_rtl_cone_slope, // 137 + + // + // 140: Sensor parameters + // + k_param_imu = 140, // deprecated - can be deleted + k_param_battery_monitoring = 141, // deprecated - can be deleted + k_param_volt_div_ratio, // deprecated - can be deleted + k_param_curr_amp_per_volt, // deprecated - can be deleted + k_param_input_voltage, // deprecated - can be deleted + k_param_pack_capacity, // deprecated - can be deleted + k_param_compass_enabled_deprecated, + k_param_compass, + k_param_rangefinder_enabled_old, // deprecated + k_param_frame_type, + k_param_optflow_enabled, // deprecated + k_param_fs_batt_voltage, // unused - moved to AP_BattMonitor + k_param_ch7_option_old, + k_param_auto_slew_rate, // deprecated - can be deleted + k_param_rangefinder_type_old, // deprecated + k_param_axis_enabled = 157, // deprecated - remove with next eeprom number change + k_param_copter_leds_mode, // deprecated - remove with next eeprom number change + k_param_ahrs, // AHRS group // 159 + + // + // 160: Navigation parameters + // + k_param_rtl_altitude = 160, + k_param_crosstrack_gain, // deprecated - remove with next eeprom number change + k_param_rtl_loiter_time, + k_param_rtl_alt_final, + k_param_tilt_comp, // 164 deprecated - remove with next eeprom number change + + + // + // Camera and mount parameters + // + k_param_camera = 165, + k_param_camera_mount, + k_param_camera_mount2, // deprecated + + // + // Battery monitoring parameters + // + k_param_battery_volt_pin = 168, // deprecated - can be deleted + k_param_battery_curr_pin, // 169 deprecated - can be deleted + + // + // 170: Radio settings + // + k_param_rc_1_old = 170, + k_param_rc_2_old, + k_param_rc_3_old, + k_param_rc_4_old, + k_param_rc_5_old, + k_param_rc_6_old, + k_param_rc_7_old, + k_param_rc_8_old, + k_param_rc_10_old, + k_param_rc_11_old, + k_param_throttle_min, // remove + k_param_throttle_max, // remove + k_param_failsafe_throttle, + k_param_throttle_fs_action, // remove + k_param_failsafe_throttle_value, + k_param_throttle_trim, // remove + k_param_radio_tuning, + k_param_radio_tuning_high_old, // unused + k_param_radio_tuning_low_old, // unused + k_param_rc_speed = 192, + k_param_failsafe_battery_enabled, // unused - moved to AP_BattMonitor + k_param_throttle_mid, // remove + k_param_failsafe_gps_enabled, // remove + k_param_rc_9_old, + k_param_rc_12_old, + k_param_failsafe_gcs, + k_param_rcmap, // 199 + + // + // 200: flight modes + // + k_param_flight_mode1 = 200, + k_param_flight_mode2, + k_param_flight_mode3, + k_param_flight_mode4, + k_param_flight_mode5, + k_param_flight_mode6, + k_param_flight_mode_chan, + k_param_initial_mode, + + // + // 210: Waypoint data + // + k_param_waypoint_mode = 210, // remove + k_param_command_total, // remove + k_param_command_index, // remove + k_param_command_nav_index, // remove + k_param_waypoint_radius, // remove + k_param_circle_radius, // remove + k_param_waypoint_speed_max, // remove + k_param_land_speed, + k_param_auto_velocity_z_min, // remove + k_param_auto_velocity_z_max, // remove - 219 + k_param_land_speed_high, + + // + // 220: PI/D Controllers + // + k_param_acro_rp_p = 221, + k_param_axis_lock_p, // remove + k_param_pid_rate_roll, // remove + k_param_pid_rate_pitch, // remove + k_param_pid_rate_yaw, // remove + k_param_p_stabilize_roll, // remove + k_param_p_stabilize_pitch, // remove + k_param_p_stabilize_yaw, // remove + k_param_p_pos_xy, // remove + k_param_p_loiter_lon, // remove + k_param_pid_loiter_rate_lat, // remove + k_param_pid_loiter_rate_lon, // remove + k_param_pid_nav_lat, // remove + k_param_pid_nav_lon, // remove + k_param_p_alt_hold, // remove + k_param_p_vel_z, // remove + k_param_pid_optflow_roll, // remove + k_param_pid_optflow_pitch, // remove + k_param_acro_balance_roll_old, // remove + k_param_acro_balance_pitch_old, // remove + k_param_pid_accel_z, // remove + k_param_acro_balance_roll, + k_param_acro_balance_pitch, + k_param_acro_yaw_p, + k_param_autotune_axis_bitmask, // remove + k_param_autotune_aggressiveness, // remove + k_param_pi_vel_xy, // remove + k_param_fs_ekf_action, + k_param_rtl_climb_min, + k_param_rpm_sensor, + k_param_autotune_min_d, // remove + k_param_arming, // 252 - AP_Arming + k_param_logger = 253, // 253 - Logging Group + + // 254,255: reserved + + k_param_vehicle = 257, // vehicle common block of parameters + + // the k_param_* space is 9-bits in size + // 511: reserved + }; + + AP_Int16 format_version; + + // Telemetry control + // + AP_Int16 sysid_this_mav; + AP_Int16 sysid_my_gcs; + AP_Int8 telem_delay; + + AP_Float throttle_filt; + AP_Int16 throttle_behavior; + AP_Float pilot_takeoff_alt; + + AP_Int16 rtl_altitude; + AP_Int16 rtl_speed_cms; + AP_Float rtl_cone_slope; + + AP_Int8 failsafe_gcs; // ground station failsafe behavior + AP_Int16 gps_hdop_good; // GPS Hdop value at or below this value represent a good position + + AP_Int16 rtl_alt_final; + AP_Int16 rtl_climb_min; // rtl minimum climb in cm + + AP_Int8 wp_yaw_behavior; // controls how the autopilot controls yaw during missions + + AP_Int16 poshold_brake_rate; // PosHold flight mode's rotation rate during braking in deg/sec + AP_Int16 poshold_brake_angle_max; // PosHold flight mode's max lean angle during braking in centi-degrees + + // Waypoints + // + AP_Int32 rtl_loiter_time; + AP_Int16 land_speed; + AP_Int16 land_speed_high; + AP_Int16 pilot_speed_up; // maximum vertical ascending velocity the pilot may request + AP_Int16 pilot_accel_z; // vertical acceleration the pilot may request + + // Throttle + // + AP_Int8 failsafe_throttle; + AP_Int16 failsafe_throttle_value; + AP_Int16 throttle_deadzone; + + // Flight modes + // + AP_Int8 flight_mode1; + AP_Int8 flight_mode2; + AP_Int8 flight_mode3; + AP_Int8 flight_mode4; + AP_Int8 flight_mode5; + AP_Int8 flight_mode6; + AP_Int8 flight_mode_chan; + AP_Int8 initial_mode; + + // Misc + // + AP_Int32 log_bitmask; + AP_Int8 radio_tuning; + AP_Int8 frame_type; + AP_Int8 disarm_delay; + + AP_Int8 land_repositioning; + AP_Int8 fs_ekf_action; + AP_Int8 fs_crash_check; + AP_Float fs_ekf_thresh; + AP_Int16 gcs_pid_mask; + + AP_Int8 rtl_alt_type; + + AP_Int16 rc_speed; // speed of fast RC Channels in Hz + + // Acro parameters + AP_Float acro_rp_p; + AP_Float acro_yaw_p; + AP_Float acro_balance_roll; + AP_Float acro_balance_pitch; + AP_Int8 acro_trainer; + AP_Float acro_rp_expo; + + // Note: keep initializers here in the same order as they are declared + // above. + Parameters() + { + } +}; + +/* + 2nd block of parameters, to avoid going past 256 top level keys + */ +class ParametersG2 +{ +public: + ParametersG2(void); + + // var_info for holding Parameter information + static const struct AP_Param::GroupInfo var_info[]; + + // altitude at which nav control can start in takeoff + AP_Float wp_navalt_min; + + // // button checking + // AP_Button *button_ptr; + +#if STATS_ENABLED == ENABLED + // vehicle statistics + AP_Stats stats; +#endif + + + // ground effect compensation enable/disable + // AP_Int8 gndeffect_comp_enabled; + + // temperature calibration handling + // AP_TempCalibration temp_calibration; + + + // whether to enforce acceptance of packets only from sysid_my_gcs + AP_Int8 sysid_enforce; + + // developer options + AP_Int32 dev_options; + + // acro exponent parameters + AP_Float acro_y_expo; + + // frame class + AP_Int8 frame_class; + + // RC input channels + RC_Channels_Blimp rc_channels; + + // control over servo output ranges + SRV_Channels servo_channels; + + // Additional pilot velocity items + AP_Int16 pilot_speed_dn; + + // Land alt final stage + AP_Int16 land_alt_low; + + +#ifdef ENABLE_SCRIPTING + AP_Scripting scripting; +#endif // ENABLE_SCRIPTING + + AP_Float tuning_min; + AP_Float tuning_max; + + // vibration failsafe enable/disable + AP_Int8 fs_vibe_enabled; + + // Failsafe options bitmask #36 + AP_Int32 fs_options; + + AP_Float fs_gcs_timeout; +}; + +extern const AP_Param::Info var_info[]; diff --git a/Blimp/RC_Channel.cpp b/Blimp/RC_Channel.cpp new file mode 100644 index 0000000000..0d8d9c7b4a --- /dev/null +++ b/Blimp/RC_Channel.cpp @@ -0,0 +1,255 @@ +#include "Blimp.h" + +#include "RC_Channel.h" + + +// defining these two macros and including the RC_Channels_VarInfo header defines the parameter information common to all vehicle types +#define RC_CHANNELS_SUBCLASS RC_Channels_Blimp +#define RC_CHANNEL_SUBCLASS RC_Channel_Blimp + +#include + +int8_t RC_Channels_Blimp::flight_mode_channel_number() const +{ + return blimp.g.flight_mode_chan.get(); +} + +void RC_Channel_Blimp::mode_switch_changed(modeswitch_pos_t new_pos) +{ + if (new_pos < 0 || (uint8_t)new_pos > blimp.num_flight_modes) { + // should not have been called + return; + } + + if (!blimp.set_mode((Mode::Number)blimp.flight_modes[new_pos].get(), ModeReason::RC_COMMAND)) { + // alert user to mode change failure + if (blimp.ap.initialised) { + AP_Notify::events.user_mode_change_failed = 1; + } + return; + } + + // play a tone + // alert user to mode change (except if autopilot is just starting up) + if (blimp.ap.initialised) { + AP_Notify::events.user_mode_change = 1; + } +} + +bool RC_Channels_Blimp::has_valid_input() const +{ + if (blimp.failsafe.radio) { + return false; + } + if (blimp.failsafe.radio_counter != 0) { + return false; + } + return true; +} + +RC_Channel * RC_Channels_Blimp::get_arming_channel(void) const +{ + return blimp.channel_yaw; +} + +// init_aux_switch_function - initialize aux functions +void RC_Channel_Blimp::init_aux_function(const aux_func_t ch_option, const AuxSwitchPos ch_flag) +{ + // init channel options + switch (ch_option) { + // the following functions do not need to be initialised: + case AUX_FUNC::MANUAL: + break; + default: + RC_Channel::init_aux_function(ch_option, ch_flag); + break; + } +} + +// do_aux_function_change_mode - change mode based on an aux switch +// being moved +void RC_Channel_Blimp::do_aux_function_change_mode(const Mode::Number mode, + const AuxSwitchPos ch_flag) +{ + switch (ch_flag) { + case AuxSwitchPos::HIGH: { + // engage mode (if not possible we remain in current flight mode) + const bool success = blimp.set_mode(mode, ModeReason::RC_COMMAND); + if (blimp.ap.initialised) { + if (success) { + AP_Notify::events.user_mode_change = 1; + } else { + AP_Notify::events.user_mode_change_failed = 1; + } + } + break; + } + default: + // return to flight mode switch's flight mode if we are currently + // in this mode + if (blimp.control_mode == mode) { + rc().reset_mode_switch(); + } + } +} + +void RC_Channel_Blimp::do_aux_function_armdisarm(const AuxSwitchPos ch_flag) +{ + RC_Channel::do_aux_function_armdisarm(ch_flag); + if (blimp.arming.is_armed()) { + // remember that we are using an arming switch, for use by + // set_throttle_zero_flag + blimp.ap.armed_with_switch = true; + } +} + +// do_aux_function - implement the function invoked by auxiliary switches +void RC_Channel_Blimp::do_aux_function(const aux_func_t ch_option, const AuxSwitchPos ch_flag) +{ + switch (ch_option) { + + case AUX_FUNC::SAVE_TRIM: + if ((ch_flag == AuxSwitchPos::HIGH) && + (blimp.control_mode <= Mode::Number::MANUAL) && + (blimp.channel_down->get_control_in() == 0)) { + blimp.save_trim(); + } + break; + + // case AUX_FUNC::SAVE_WP: + // #if MODE_AUTO_ENABLED == ENABLED + // // save waypoint when switch is brought high + // if (ch_flag == RC_Channel::AuxSwitchPos::HIGH) { + + // // do not allow saving new waypoints while we're in auto or disarmed + // if (blimp.control_mode == Mode::Number::AUTO || !blimp.motors->armed()) { + // return; + // } + + // // do not allow saving the first waypoint with zero throttle + // if ((blimp.mode_auto.mission.num_commands() == 0) && (blimp.channel_down->get_control_in() == 0)) { + // return; + // } + + // // create new mission command + // AP_Mission::Mission_Command cmd = {}; + + // // if the mission is empty save a takeoff command + // if (blimp.mode_auto.mission.num_commands() == 0) { + // // set our location ID to 16, MAV_CMD_NAV_WAYPOINT + // cmd.id = MAV_CMD_NAV_TAKEOFF; + // cmd.content.location.alt = MAX(blimp.current_loc.alt,100); + + // // use the current altitude for the target alt for takeoff. + // // only altitude will matter to the AP mission script for takeoff. + // if (blimp.mode_auto.mission.add_cmd(cmd)) { + // // log event + // AP::logger().Write_Event(LogEvent::SAVEWP_ADD_WP); + // } + // } + + // // set new waypoint to current location + // cmd.content.location = blimp.current_loc; + + // // if throttle is above zero, create waypoint command + // if (blimp.channel_down->get_control_in() > 0) { + // cmd.id = MAV_CMD_NAV_WAYPOINT; + // } else { + // // with zero throttle, create LAND command + // cmd.id = MAV_CMD_NAV_LAND; + // } + + // // save command + // if (blimp.mode_auto.mission.add_cmd(cmd)) { + // // log event + // AP::logger().Write_Event(LogEvent::SAVEWP_ADD_WP); + // } + // } + // #endif + // break; + + // case AUX_FUNC::AUTO: + // #if MODE_AUTO_ENABLED == ENABLED + // do_aux_function_change_mode(Mode::Number::AUTO, ch_flag); + // #endif + // break; + + // case AUX_FUNC::LOITER: + // do_aux_function_change_mode(Mode::Number::LOITER, ch_flag); + // break; + + case AUX_FUNC::MANUAL: + do_aux_function_change_mode(Mode::Number::MANUAL, ch_flag); + break; + + default: + RC_Channel::do_aux_function(ch_option, ch_flag); + break; + } +} + +// save_trim - adds roll and pitch trims from the radio to ahrs +void Blimp::save_trim() +{ + // save roll and pitch trim + float roll_trim = ToRad((float)channel_right->get_control_in()/100.0f); + float pitch_trim = ToRad((float)channel_front->get_control_in()/100.0f); + ahrs.add_trim(roll_trim, pitch_trim); + AP::logger().Write_Event(LogEvent::SAVE_TRIM); + gcs().send_text(MAV_SEVERITY_INFO, "Trim saved"); +} + +// auto_trim - slightly adjusts the ahrs.roll_trim and ahrs.pitch_trim towards the current stick positions +// meant to be called continuously while the pilot attempts to keep the blimp level +void Blimp::auto_trim_cancel() +{ + auto_trim_counter = 0; + AP_Notify::flags.save_trim = false; + gcs().send_text(MAV_SEVERITY_INFO, "AutoTrim cancelled"); +} + +void Blimp::auto_trim() +{ + if (auto_trim_counter > 0) { + if (blimp.flightmode != &blimp.mode_manual || + !blimp.motors->armed()) { + auto_trim_cancel(); + return; + } + + // flash the leds + AP_Notify::flags.save_trim = true; + + if (!auto_trim_started) { + if (ap.land_complete) { + // haven't taken off yet + return; + } + auto_trim_started = true; + } + + if (ap.land_complete) { + // landed again. + auto_trim_cancel(); + return; + } + + auto_trim_counter--; + + // calculate roll trim adjustment + float roll_trim_adjustment = ToRad((float)channel_right->get_control_in() / 4000.0f); + + // calculate pitch trim adjustment + float pitch_trim_adjustment = ToRad((float)channel_front->get_control_in() / 4000.0f); + + // add trim to ahrs object + // save to eeprom on last iteration + ahrs.add_trim(roll_trim_adjustment, pitch_trim_adjustment, (auto_trim_counter == 0)); + + // on last iteration restore leds and accel gains to normal + if (auto_trim_counter == 0) { + AP_Notify::flags.save_trim = false; + gcs().send_text(MAV_SEVERITY_INFO, "AutoTrim: Trims saved"); + } + } +} diff --git a/Blimp/RC_Channel.h b/Blimp/RC_Channel.h new file mode 100644 index 0000000000..7734813cfc --- /dev/null +++ b/Blimp/RC_Channel.h @@ -0,0 +1,50 @@ +#pragma once + +#include +#include "Fins.h" +#include "mode.h" //this includes Blimp.h which includes Fins.h + +class RC_Channel_Blimp : public RC_Channel +{ + +public: + +protected: + + void init_aux_function(aux_func_t ch_option, AuxSwitchPos) override; + void do_aux_function(aux_func_t ch_option, AuxSwitchPos) override; + +private: + + void do_aux_function_armdisarm(const AuxSwitchPos ch_flag) override; + void do_aux_function_change_mode(const Mode::Number mode, + const AuxSwitchPos ch_flag); + void do_aux_function_change_air_mode(const AuxSwitchPos ch_flag); + + // called when the mode switch changes position: + void mode_switch_changed(modeswitch_pos_t new_pos) override; + +}; + +class RC_Channels_Blimp : public RC_Channels +{ +public: + + bool has_valid_input() const override; + + RC_Channel *get_arming_channel(void) const override; + + RC_Channel_Blimp obj_channels[NUM_RC_CHANNELS]; + RC_Channel_Blimp *channel(const uint8_t chan) override + { + if (chan >= NUM_RC_CHANNELS) { + return nullptr; + } + return &obj_channels[chan]; + } + +protected: + + int8_t flight_mode_channel_number() const override; + +}; diff --git a/Blimp/commands.cpp b/Blimp/commands.cpp new file mode 100644 index 0000000000..537cf2e1e1 --- /dev/null +++ b/Blimp/commands.cpp @@ -0,0 +1,110 @@ +#include "Blimp.h" + +// checks if we should update ahrs/RTL home position from the EKF +void Blimp::update_home_from_EKF() +{ + // exit immediately if home already set + if (ahrs.home_is_set()) { + return; + } + + // special logic if home is set in-flight + if (motors->armed()) { + set_home_to_current_location_inflight(); + } else { + // move home to current ekf location (this will set home_state to HOME_SET) + if (!set_home_to_current_location(false)) { + // ignore failure + } + } +} + +// set_home_to_current_location_inflight - set home to current GPS location (horizontally) and EKF origin vertically +void Blimp::set_home_to_current_location_inflight() +{ + // get current location from EKF + Location temp_loc; + Location ekf_origin; + if (ahrs.get_location(temp_loc) && ahrs.get_origin(ekf_origin)) { + temp_loc.alt = ekf_origin.alt; + if (!set_home(temp_loc, false)) { + return; + } + } +} + +// set_home_to_current_location - set home to current GPS location +bool Blimp::set_home_to_current_location(bool lock) +{ + // get current location from EKF + Location temp_loc; + if (ahrs.get_location(temp_loc)) { + if (!set_home(temp_loc, lock)) { + return false; + } + return true; + } + return false; +} + +// set_home - sets ahrs home (used for RTL) to specified location +// initialises inertial nav and compass on first call +// returns true if home location set successfully +bool Blimp::set_home(const Location& loc, bool lock) +{ + // check EKF origin has been set + Location ekf_origin; + if (!ahrs.get_origin(ekf_origin)) { + return false; + } + + // check home is close to EKF origin + if (far_from_EKF_origin(loc)) { + return false; + } + + const bool home_was_set = ahrs.home_is_set(); + + // set ahrs home (used for RTL) + if (!ahrs.set_home(loc)) { + return false; + } + + // init inav and compass declination + if (!home_was_set) { + // record home is set + AP::logger().Write_Event(LogEvent::SET_HOME); + + // #if MODE_AUTO_ENABLED == ENABLED + // // log new home position which mission library will pull from ahrs + // if (should_log(MASK_LOG_CMD)) { + // AP_Mission::Mission_Command temp_cmd; + // if (mode_auto.mission.read_cmd_from_storage(0, temp_cmd)) { + // logger.Write_Mission_Cmd(mode_auto.mission, temp_cmd); + // } + // } + // #endif + } + + // lock home position + if (lock) { + ahrs.lock_home(); + } + + // return success + return true; +} + +// far_from_EKF_origin - checks if a location is too far from the EKF origin +// returns true if too far +bool Blimp::far_from_EKF_origin(const Location& loc) +{ + // check distance to EKF origin + Location ekf_origin; + if (ahrs.get_origin(ekf_origin) && ((ekf_origin.get_distance(loc) > EKF_ORIGIN_MAX_DIST_M) || (labs(ekf_origin.alt - loc.alt) > EKF_ORIGIN_MAX_DIST_M))) { + return true; + } + + // close enough to origin + return false; +} diff --git a/Blimp/config.h b/Blimp/config.h new file mode 100644 index 0000000000..63464f24e2 --- /dev/null +++ b/Blimp/config.h @@ -0,0 +1,593 @@ +// +#pragma once + +////////////////////////////////////////////////////////////////////////////// +// +// Default and automatic configuration details. +// +// +#include "defines.h" + +////////////////////////////////////////////////////////////////////////////// +////////////////////////////////////////////////////////////////////////////// +// HARDWARE CONFIGURATION AND CONNECTIONS +////////////////////////////////////////////////////////////////////////////// +////////////////////////////////////////////////////////////////////////////// + +#ifndef CONFIG_HAL_BOARD +#error CONFIG_HAL_BOARD must be defined to build Blimp +#endif + +#ifndef ADVANCED_FAILSAFE +# define ADVANCED_FAILSAFE DISABLED +#endif + +////////////////////////////////////////////////////////////////////////////// +// HIL_MODE OPTIONAL + +#ifndef HIL_MODE +#define HIL_MODE HIL_MODE_DISABLED +#endif + +#ifndef ARMING_DELAY_SEC +# define ARMING_DELAY_SEC 2.0f +#endif + +////////////////////////////////////////////////////////////////////////////// +// FRAME_CONFIG +// +#ifndef FRAME_CONFIG +# define FRAME_CONFIG MULTICOPTER_FRAME +#endif + +////////////////////////////////////////////////////////////////////////////// +// PWM control +// default RC speed in Hz +#ifndef RC_FAST_SPEED +# define RC_FAST_SPEED 490 +#endif + +////////////////////////////////////////////////////////////////////////////// +// Rangefinder +// + +#ifndef RANGEFINDER_ENABLED +# define RANGEFINDER_ENABLED ENABLED +#endif + +#ifndef RANGEFINDER_HEALTH_MAX +# define RANGEFINDER_HEALTH_MAX 3 // number of good reads that indicates a healthy rangefinder +#endif + +#ifndef RANGEFINDER_TIMEOUT_MS +# define RANGEFINDER_TIMEOUT_MS 1000 // rangefinder filter reset if no updates from sensor in 1 second +#endif + +#ifndef RANGEFINDER_GAIN_DEFAULT +# define RANGEFINDER_GAIN_DEFAULT 0.8f // gain for controlling how quickly rangefinder range adjusts target altitude (lower means slower reaction) +#endif + +#ifndef SURFACE_TRACKING_VELZ_MAX +# define SURFACE_TRACKING_VELZ_MAX 150 // max vertical speed change while surface tracking with rangefinder +#endif + +#ifndef SURFACE_TRACKING_TIMEOUT_MS +# define SURFACE_TRACKING_TIMEOUT_MS 1000 // surface tracking target alt will reset to current rangefinder alt after this many milliseconds without a good rangefinder alt +#endif + +#ifndef RANGEFINDER_WPNAV_FILT_HZ +# define RANGEFINDER_WPNAV_FILT_HZ 0.25f // filter frequency for rangefinder altitude provided to waypoint navigation class +#endif + +#ifndef RANGEFINDER_TILT_CORRECTION // by disable tilt correction for use of range finder data by EKF +# define RANGEFINDER_TILT_CORRECTION ENABLED +#endif + +#ifndef RANGEFINDER_GLITCH_ALT_CM +# define RANGEFINDER_GLITCH_ALT_CM 200 // amount of rangefinder change to be considered a glitch +#endif + +#ifndef RANGEFINDER_GLITCH_NUM_SAMPLES +# define RANGEFINDER_GLITCH_NUM_SAMPLES 3 // number of rangefinder glitches in a row to take new reading +#endif + +////////////////////////////////////////////////////////////////////////////// +// Proximity sensor +// +#ifndef PROXIMITY_ENABLED +# define PROXIMITY_ENABLED ENABLED +#endif + +#ifndef MAV_SYSTEM_ID +# define MAV_SYSTEM_ID 1 +#endif + + +////////////////////////////////////////////////////////////////////////////// +// Battery monitoring +// +#ifndef BOARD_VOLTAGE_MIN +# define BOARD_VOLTAGE_MIN 4.3f // min board voltage in volts for pre-arm checks +#endif + +#ifndef BOARD_VOLTAGE_MAX +# define BOARD_VOLTAGE_MAX 5.8f // max board voltage in volts for pre-arm checks +#endif + +// prearm GPS hdop check +#ifndef GPS_HDOP_GOOD_DEFAULT +# define GPS_HDOP_GOOD_DEFAULT 140 // minimum hdop that represents a good position. used during pre-arm checks if fence is enabled +#endif + +// GCS failsafe +#ifndef FS_GCS +# define FS_GCS DISABLED +#endif + +// Radio failsafe while using RC_override +#ifndef FS_RADIO_RC_OVERRIDE_TIMEOUT_MS +# define FS_RADIO_RC_OVERRIDE_TIMEOUT_MS 1000 // RC Radio failsafe triggers after 1 second while using RC_override from ground station +#endif + +// Radio failsafe +#ifndef FS_RADIO_TIMEOUT_MS +#define FS_RADIO_TIMEOUT_MS 500 // RC Radio Failsafe triggers after 500 milliseconds with No RC Input +#endif + +// missing terrain data failsafe +#ifndef FS_TERRAIN_TIMEOUT_MS +#define FS_TERRAIN_TIMEOUT_MS 5000 // 5 seconds of missing terrain data will trigger failsafe (RTL) +#endif + +#ifndef PREARM_DISPLAY_PERIOD +# define PREARM_DISPLAY_PERIOD 30 +#endif + +// pre-arm baro vs inertial nav max alt disparity +#ifndef PREARM_MAX_ALT_DISPARITY_CM +# define PREARM_MAX_ALT_DISPARITY_CM 100 // barometer and inertial nav altitude must be within this many centimeters +#endif + +////////////////////////////////////////////////////////////////////////////// +// EKF Failsafe +#ifndef FS_EKF_ACTION_DEFAULT +# define FS_EKF_ACTION_DEFAULT FS_EKF_ACTION_LAND // EKF failsafe triggers land by default +#endif +#ifndef FS_EKF_THRESHOLD_DEFAULT +# define FS_EKF_THRESHOLD_DEFAULT 0.8f // EKF failsafe's default compass and velocity variance threshold above which the EKF failsafe will be triggered +#endif + +#ifndef EKF_ORIGIN_MAX_DIST_M +# define EKF_ORIGIN_MAX_DIST_M 50000 // EKF origin and waypoints (including home) must be within 50km +#endif + +#ifndef COMPASS_CAL_STICK_GESTURE_TIME +#define COMPASS_CAL_STICK_GESTURE_TIME 2.0f // 2 seconds +#endif +#ifndef COMPASS_CAL_STICK_DELAY +#define COMPASS_CAL_STICK_DELAY 5.0f +#endif + +////////////////////////////////////////////////////////////////////////////// +// OPTICAL_FLOW +#ifndef OPTFLOW +# define OPTFLOW ENABLED +#endif + +////////////////////////////////////////////////////////////////////////////// +// Auto Tuning +#ifndef AUTOTUNE_ENABLED +# define AUTOTUNE_ENABLED ENABLED +#endif + +////////////////////////////////////////////////////////////////////////////// +// Crop Sprayer - enabled only on larger firmwares +#ifndef SPRAYER_ENABLED +# define SPRAYER_ENABLED HAL_SPRAYER_ENABLED +#endif + +////////////////////////////////////////////////////////////////////////////// +// Precision Landing with companion computer or IRLock sensor +#ifndef PRECISION_LANDING +# define PRECISION_LANDING ENABLED +#endif + +////////////////////////////////////////////////////////////////////////////// +// gripper - enabled only on larger firmwares +#ifndef GRIPPER_ENABLED +# define GRIPPER_ENABLED !HAL_MINIMIZE_FEATURES +#endif + +////////////////////////////////////////////////////////////////////////////// +// rotations per minute sensor support +#ifndef RPM_ENABLED +# define RPM_ENABLED !HAL_MINIMIZE_FEATURES +#endif + +////////////////////////////////////////////////////////////////////////////// +// Parachute release +#ifndef PARACHUTE +# define PARACHUTE HAL_PARACHUTE_ENABLED +#endif + +////////////////////////////////////////////////////////////////////////////// +// Nav-Guided - allows external nav computer to control vehicle +#ifndef NAV_GUIDED +# define NAV_GUIDED !HAL_MINIMIZE_FEATURES +#endif + +////////////////////////////////////////////////////////////////////////////// +// Acro - fly vehicle in acrobatic mode +#ifndef MODE_ACRO_ENABLED +# define MODE_ACRO_ENABLED ENABLED +#endif + +////////////////////////////////////////////////////////////////////////////// +// Auto mode - allows vehicle to trace waypoints and perform automated actions +#ifndef MODE_AUTO_ENABLED +# define MODE_AUTO_ENABLED ENABLED +#endif + +////////////////////////////////////////////////////////////////////////////// +// Brake mode - bring vehicle to stop +#ifndef MODE_BRAKE_ENABLED +# define MODE_BRAKE_ENABLED ENABLED +#endif + +////////////////////////////////////////////////////////////////////////////// +// Circle - fly vehicle around a central point +#ifndef MODE_CIRCLE_ENABLED +# define MODE_CIRCLE_ENABLED ENABLED +#endif + +////////////////////////////////////////////////////////////////////////////// +// Drift - fly vehicle in altitude-held, coordinated-turn mode +#ifndef MODE_DRIFT_ENABLED +# define MODE_DRIFT_ENABLED ENABLED +#endif + +////////////////////////////////////////////////////////////////////////////// +// flip - fly vehicle in flip in pitch and roll direction mode +#ifndef MODE_FLIP_ENABLED +# define MODE_FLIP_ENABLED ENABLED +#endif + +////////////////////////////////////////////////////////////////////////////// +// Follow - follow another vehicle or GCS +#ifndef MODE_FOLLOW_ENABLED +# define MODE_FOLLOW_ENABLED !HAL_MINIMIZE_FEATURES +#endif + +////////////////////////////////////////////////////////////////////////////// +// Guided mode - control vehicle's position or angles from GCS +#ifndef MODE_GUIDED_ENABLED +# define MODE_GUIDED_ENABLED ENABLED +#endif + +////////////////////////////////////////////////////////////////////////////// +// GuidedNoGPS mode - control vehicle's angles from GCS +#ifndef MODE_GUIDED_NOGPS_ENABLED +# define MODE_GUIDED_NOGPS_ENABLED !HAL_MINIMIZE_FEATURES +#endif + +////////////////////////////////////////////////////////////////////////////// +// Loiter mode - allows vehicle to hold global position +#ifndef MODE_LOITER_ENABLED +# define MODE_LOITER_ENABLED ENABLED +#endif + +////////////////////////////////////////////////////////////////////////////// +// Position Hold - enable holding of global position +#ifndef MODE_POSHOLD_ENABLED +# define MODE_POSHOLD_ENABLED ENABLED +#endif + +////////////////////////////////////////////////////////////////////////////// +// RTL - Return To Launch +#ifndef MODE_RTL_ENABLED +# define MODE_RTL_ENABLED ENABLED +#endif + +////////////////////////////////////////////////////////////////////////////// +// SmartRTL - allows vehicle to retrace a (loop-eliminated) breadcrumb home +#ifndef MODE_SMARTRTL_ENABLED +# define MODE_SMARTRTL_ENABLED ENABLED +#endif + +////////////////////////////////////////////////////////////////////////////// +// Sport - fly vehicle in rate-controlled (earth-frame) mode +#ifndef MODE_SPORT_ENABLED +# define MODE_SPORT_ENABLED !HAL_MINIMIZE_FEATURES +#endif + +////////////////////////////////////////////////////////////////////////////// +// System ID - conduct system identification tests on vehicle +#ifndef MODE_SYSTEMID_ENABLED +# define MODE_SYSTEMID_ENABLED !HAL_MINIMIZE_FEATURES +#endif + +////////////////////////////////////////////////////////////////////////////// +// Button - Enable the button connected to AUX1-6 +#ifndef BUTTON_ENABLED +# define BUTTON_ENABLED ENABLED +#endif + +////////////////////////////////////////////////////////////////////////////// +// RADIO CONFIGURATION +////////////////////////////////////////////////////////////////////////////// +////////////////////////////////////////////////////////////////////////////// + + +////////////////////////////////////////////////////////////////////////////// +// FLIGHT_MODE +// + +#ifndef FLIGHT_MODE_1 +# define FLIGHT_MODE_1 Mode::Number::MANUAL +#endif +#ifndef FLIGHT_MODE_2 +# define FLIGHT_MODE_2 Mode::Number::MANUAL +#endif +#ifndef FLIGHT_MODE_3 +# define FLIGHT_MODE_3 Mode::Number::MANUAL +#endif +#ifndef FLIGHT_MODE_4 +# define FLIGHT_MODE_4 Mode::Number::MANUAL +#endif +#ifndef FLIGHT_MODE_5 +# define FLIGHT_MODE_5 Mode::Number::MANUAL +#endif +#ifndef FLIGHT_MODE_6 +# define FLIGHT_MODE_6 Mode::Number::MANUAL +#endif + + +////////////////////////////////////////////////////////////////////////////// +// Throttle Failsafe +// +#ifndef FS_THR_VALUE_DEFAULT +# define FS_THR_VALUE_DEFAULT 975 +#endif + +////////////////////////////////////////////////////////////////////////////// +// Takeoff +// +#ifndef PILOT_TKOFF_ALT_DEFAULT +# define PILOT_TKOFF_ALT_DEFAULT 0 // default final alt above home for pilot initiated takeoff +#endif + + +////////////////////////////////////////////////////////////////////////////// +// Landing +// +#ifndef LAND_SPEED +# define LAND_SPEED 50 // the descent speed for the final stage of landing in cm/s +#endif +#ifndef LAND_REPOSITION_DEFAULT +# define LAND_REPOSITION_DEFAULT 1 // by default the pilot can override roll/pitch during landing +#endif +#ifndef LAND_WITH_DELAY_MS +# define LAND_WITH_DELAY_MS 4000 // default delay (in milliseconds) when a land-with-delay is triggered during a failsafe event +#endif +#ifndef LAND_CANCEL_TRIGGER_THR +# define LAND_CANCEL_TRIGGER_THR 700 // land is cancelled by input throttle above 700 +#endif +#ifndef LAND_RANGEFINDER_MIN_ALT_CM +#define LAND_RANGEFINDER_MIN_ALT_CM 200 +#endif + +////////////////////////////////////////////////////////////////////////////// +// Landing Detector +// +#ifndef LAND_DETECTOR_TRIGGER_SEC +# define LAND_DETECTOR_TRIGGER_SEC 1.0f // number of seconds to detect a landing +#endif +#ifndef LAND_DETECTOR_MAYBE_TRIGGER_SEC +# define LAND_DETECTOR_MAYBE_TRIGGER_SEC 0.2f // number of seconds that means we might be landed (used to reset horizontal position targets to prevent tipping over) +#endif +#ifndef LAND_DETECTOR_ACCEL_LPF_CUTOFF +# define LAND_DETECTOR_ACCEL_LPF_CUTOFF 1.0f // frequency cutoff of land detector accelerometer filter +#endif +#ifndef LAND_DETECTOR_ACCEL_MAX +# define LAND_DETECTOR_ACCEL_MAX 1.0f // vehicle acceleration must be under 1m/s/s +#endif + +////////////////////////////////////////////////////////////////////////////// +// CAMERA TRIGGER AND CONTROL +// +#ifndef CAMERA +# define CAMERA ENABLED +#endif + +////////////////////////////////////////////////////////////////////////////// +// Flight mode definitions +// + +// Acro Mode +#ifndef ACRO_RP_P +# define ACRO_RP_P 4.5f +#endif + +#ifndef ACRO_YAW_P +# define ACRO_YAW_P 4.5f +#endif + +#ifndef ACRO_LEVEL_MAX_ANGLE +# define ACRO_LEVEL_MAX_ANGLE 3000 +#endif + +#ifndef ACRO_BALANCE_ROLL +#define ACRO_BALANCE_ROLL 1.0f +#endif + +#ifndef ACRO_BALANCE_PITCH +#define ACRO_BALANCE_PITCH 1.0f +#endif + +#ifndef ACRO_RP_EXPO_DEFAULT +#define ACRO_RP_EXPO_DEFAULT 0.3f +#endif + +#ifndef ACRO_Y_EXPO_DEFAULT +#define ACRO_Y_EXPO_DEFAULT 0.0f +#endif + +#ifndef ACRO_THR_MID_DEFAULT +#define ACRO_THR_MID_DEFAULT 0.0f +#endif + +// RTL Mode +#ifndef RTL_ALT_FINAL +# define RTL_ALT_FINAL 0 // the altitude the vehicle will move to as the final stage of Returning to Launch. Set to zero to land. +#endif + +#ifndef RTL_ALT +# define RTL_ALT 1500 // default alt to return to home in cm, 0 = Maintain current altitude +#endif + +#ifndef RTL_ALT_MIN +# define RTL_ALT_MIN 200 // min height above ground for RTL (i.e 2m) +#endif + +#ifndef RTL_CLIMB_MIN_DEFAULT +# define RTL_CLIMB_MIN_DEFAULT 0 // vehicle will always climb this many cm as first stage of RTL +#endif + +#ifndef RTL_ABS_MIN_CLIMB +# define RTL_ABS_MIN_CLIMB 250 // absolute minimum initial climb +#endif + +#ifndef RTL_CONE_SLOPE_DEFAULT +# define RTL_CONE_SLOPE_DEFAULT 3.0f // slope of RTL cone (height / distance). 0 = No cone +#endif + +#ifndef RTL_MIN_CONE_SLOPE +# define RTL_MIN_CONE_SLOPE 0.5f // minimum slope of cone +#endif + +#ifndef RTL_LOITER_TIME +# define RTL_LOITER_TIME 5000 // Time (in milliseconds) to loiter above home before beginning final descent +#endif + +// AUTO Mode +#ifndef WP_YAW_BEHAVIOR_DEFAULT +# define WP_YAW_BEHAVIOR_DEFAULT WP_YAW_BEHAVIOR_LOOK_AT_NEXT_WP_EXCEPT_RTL +#endif + +#ifndef AUTO_YAW_SLEW_RATE +# define AUTO_YAW_SLEW_RATE 60 // degrees/sec +#endif + +#ifndef YAW_LOOK_AHEAD_MIN_SPEED +# define YAW_LOOK_AHEAD_MIN_SPEED 100 // minimum ground speed in cm/s required before blimp is aimed at ground course +#endif + +////////////////////////////////////////////////////////////////////////////// +// Stabilize Rate Control +// +#ifndef ROLL_PITCH_YAW_INPUT_MAX +# define ROLL_PITCH_YAW_INPUT_MAX 4500 // roll, pitch and yaw input range +#endif +#ifndef DEFAULT_ANGLE_MAX +# define DEFAULT_ANGLE_MAX 4500 // ANGLE_MAX parameters default value +#endif +#ifndef ANGLE_RATE_MAX +# define ANGLE_RATE_MAX 18000 // default maximum rotation rate in roll/pitch axis requested by angle controller used in stabilize, loiter, rtl, auto flight modes +#endif + +////////////////////////////////////////////////////////////////////////////// +// Stop mode defaults +// +#ifndef BRAKE_MODE_SPEED_Z +# define BRAKE_MODE_SPEED_Z 250 // z-axis speed in cm/s in Brake Mode +#endif +#ifndef BRAKE_MODE_DECEL_RATE +# define BRAKE_MODE_DECEL_RATE 750 // acceleration rate in cm/s/s in Brake Mode +#endif + +////////////////////////////////////////////////////////////////////////////// +// PosHold parameter defaults +// +#ifndef POSHOLD_BRAKE_RATE_DEFAULT +# define POSHOLD_BRAKE_RATE_DEFAULT 8 // default POSHOLD_BRAKE_RATE param value. Rotation rate during braking in deg/sec +#endif +#ifndef POSHOLD_BRAKE_ANGLE_DEFAULT +# define POSHOLD_BRAKE_ANGLE_DEFAULT 3000 // default POSHOLD_BRAKE_ANGLE param value. Max lean angle during braking in centi-degrees +#endif + +////////////////////////////////////////////////////////////////////////////// +// Throttle control defaults +// + +#ifndef THR_DZ_DEFAULT +# define THR_DZ_DEFAULT 100 // the deadzone above and below mid throttle while in althold or loiter +#endif + +// default maximum vertical velocity and acceleration the pilot may request +#ifndef PILOT_VELZ_MAX +# define PILOT_VELZ_MAX 250 // maximum vertical velocity in cm/s +#endif +#ifndef PILOT_ACCEL_Z_DEFAULT +# define PILOT_ACCEL_Z_DEFAULT 250 // vertical acceleration in cm/s/s while altitude is under pilot control +#endif + +// max distance in cm above or below current location that will be used for the alt target when transitioning to alt-hold mode +#ifndef ALT_HOLD_INIT_MAX_OVERSHOOT +# define ALT_HOLD_INIT_MAX_OVERSHOOT 200 +#endif +// the acceleration used to define the distance-velocity curve +#ifndef ALT_HOLD_ACCEL_MAX +# define ALT_HOLD_ACCEL_MAX 250 // if you change this you must also update the duplicate declaration in AC_WPNav.h +#endif + +#ifndef AUTO_DISARMING_DELAY +# define AUTO_DISARMING_DELAY 10 +#endif + +////////////////////////////////////////////////////////////////////////////// +// Throw mode configuration +// +#ifndef THROW_HIGH_SPEED +# define THROW_HIGH_SPEED 500.0f // vehicle much reach this total 3D speed in cm/s (or be free falling) +#endif +#ifndef THROW_VERTICAL_SPEED +# define THROW_VERTICAL_SPEED 50.0f // motors start when vehicle reaches this total 3D speed in cm/s +#endif + +////////////////////////////////////////////////////////////////////////////// +// Logging control +// +#ifndef LOGGING_ENABLED +# define LOGGING_ENABLED ENABLED +#endif + +// Default logging bitmask +#ifndef DEFAULT_LOG_BITMASK +# define DEFAULT_LOG_BITMASK \ + MASK_LOG_ATTITUDE_MED | \ + MASK_LOG_GPS | \ + MASK_LOG_PM | \ + MASK_LOG_CTUN | \ + MASK_LOG_NTUN | \ + MASK_LOG_RCIN | \ + MASK_LOG_IMU | \ + MASK_LOG_CMD | \ + MASK_LOG_CURRENT | \ + MASK_LOG_RCOUT | \ + MASK_LOG_OPTFLOW | \ + MASK_LOG_COMPASS | \ + MASK_LOG_CAMERA | \ + MASK_LOG_MOTBATT +#endif + +#ifndef CH_MODE_DEFAULT +# define CH_MODE_DEFAULT 5 +#endif + +#ifndef STATS_ENABLED +# define STATS_ENABLED ENABLED +#endif + +#ifndef HAL_FRAME_TYPE_DEFAULT +#define HAL_FRAME_TYPE_DEFAULT Fins::MOTOR_FRAME_TYPE_AIRFISH +#endif diff --git a/Blimp/defines.h b/Blimp/defines.h new file mode 100644 index 0000000000..8936dbfe27 --- /dev/null +++ b/Blimp/defines.h @@ -0,0 +1,209 @@ +#pragma once + +#include + +// Just so that it's completely clear... +#define ENABLED 1 +#define DISABLED 0 + +// this avoids a very common config error +#define ENABLE ENABLED +#define DISABLE DISABLED + +// Autopilot Yaw Mode enumeration +enum autopilot_yaw_mode { + AUTO_YAW_HOLD = 0, // pilot controls the heading + AUTO_YAW_LOOK_AT_NEXT_WP = 1, // point towards next waypoint (no pilot input accepted) + AUTO_YAW_ROI = 2, // point towards a location held in roi (no pilot input accepted) + AUTO_YAW_FIXED = 3, // point towards a particular angle (no pilot input accepted) + AUTO_YAW_LOOK_AHEAD = 4, // point in the direction the blimp is moving + AUTO_YAW_RESETTOARMEDYAW = 5, // point towards heading at time motors were armed + AUTO_YAW_RATE = 6, // turn at a specified rate (held in auto_yaw_rate) + AUTO_YAW_CIRCLE = 7, // use AC_Circle's provided yaw (used during Loiter-Turns commands) +}; + +// Frame types +#define UNDEFINED_FRAME 0 +#define MULTICOPTER_FRAME 1 +#define HELI_FRAME 2 + +// HIL enumerations +#define HIL_MODE_DISABLED 0 +#define HIL_MODE_SENSORS 1 + +// Tuning enumeration +enum tuning_func { + TUNING_NONE = 0, // + TUNING_STABILIZE_ROLL_PITCH_KP = 1, // stabilize roll/pitch angle controller's P term + TUNING_STABILIZE_YAW_KP = 3, // stabilize yaw heading controller's P term + TUNING_RATE_ROLL_PITCH_KP = 4, // body frame roll/pitch rate controller's P term + TUNING_RATE_ROLL_PITCH_KI = 5, // body frame roll/pitch rate controller's I term + TUNING_YAW_RATE_KP = 6, // body frame yaw rate controller's P term + TUNING_THROTTLE_RATE_KP = 7, // throttle rate controller's P term (desired rate to acceleration or motor output) + TUNING_WP_SPEED = 10, // maximum speed to next way point (0 to 10m/s) + TUNING_LOITER_POSITION_KP = 12, // loiter distance controller's P term (position error to speed) + TUNING_HELI_EXTERNAL_GYRO = 13, // TradHeli specific external tail gyro gain + TUNING_ALTITUDE_HOLD_KP = 14, // altitude hold controller's P term (alt error to desired rate) + TUNING_RATE_ROLL_PITCH_KD = 21, // body frame roll/pitch rate controller's D term + TUNING_VEL_XY_KP = 22, // loiter rate controller's P term (speed error to tilt angle) + TUNING_ACRO_RP_KP = 25, // acro controller's P term. converts pilot input to a desired roll, pitch or yaw rate + TUNING_YAW_RATE_KD = 26, // body frame yaw rate controller's D term + TUNING_VEL_XY_KI = 28, // loiter rate controller's I term (speed error to tilt angle) + TUNING_AHRS_YAW_KP = 30, // ahrs's compass effect on yaw angle (0 = very low, 1 = very high) + TUNING_AHRS_KP = 31, // accelerometer effect on roll/pitch angle (0=low) + TUNING_ACCEL_Z_KP = 34, // accel based throttle controller's P term + TUNING_ACCEL_Z_KI = 35, // accel based throttle controller's I term + TUNING_ACCEL_Z_KD = 36, // accel based throttle controller's D term + TUNING_DECLINATION = 38, // compass declination in radians + TUNING_CIRCLE_RATE = 39, // circle turn rate in degrees (hard coded to about 45 degrees in either direction) + TUNING_ACRO_YAW_KP = 40, // acro controller's P term. converts pilot input to a desired roll, pitch or yaw rate + TUNING_RANGEFINDER_GAIN = 41, // rangefinder gain + TUNING_EKF_VERTICAL_POS = 42, // unused + TUNING_EKF_HORIZONTAL_POS = 43, // unused + TUNING_EKF_ACCEL_NOISE = 44, // unused + TUNING_RC_FEEL_RP = 45, // roll-pitch input smoothing + TUNING_RATE_PITCH_KP = 46, // body frame pitch rate controller's P term + TUNING_RATE_PITCH_KI = 47, // body frame pitch rate controller's I term + TUNING_RATE_PITCH_KD = 48, // body frame pitch rate controller's D term + TUNING_RATE_ROLL_KP = 49, // body frame roll rate controller's P term + TUNING_RATE_ROLL_KI = 50, // body frame roll rate controller's I term + TUNING_RATE_ROLL_KD = 51, // body frame roll rate controller's D term + TUNING_RATE_PITCH_FF = 52, // body frame pitch rate controller FF term + TUNING_RATE_ROLL_FF = 53, // body frame roll rate controller FF term + TUNING_RATE_YAW_FF = 54, // body frame yaw rate controller FF term + TUNING_RATE_MOT_YAW_HEADROOM = 55, // motors yaw headroom minimum + TUNING_RATE_YAW_FILT = 56, // yaw rate input filter + UNUSED = 57, // was winch control + TUNING_SYSTEM_ID_MAGNITUDE = 58 // magnitude of the system ID signal +}; + +// Yaw behaviours during missions - possible values for WP_YAW_BEHAVIOR parameter +#define WP_YAW_BEHAVIOR_NONE 0 // auto pilot will never control yaw during missions or rtl (except for DO_CONDITIONAL_YAW command received) +#define WP_YAW_BEHAVIOR_LOOK_AT_NEXT_WP 1 // auto pilot will face next waypoint or home during rtl +#define WP_YAW_BEHAVIOR_LOOK_AT_NEXT_WP_EXCEPT_RTL 2 // auto pilot will face next waypoint except when doing RTL at which time it will stay in it's last +#define WP_YAW_BEHAVIOR_LOOK_AHEAD 3 // auto pilot will look ahead during missions and rtl (primarily meant for traditional helicopters) + +// Auto modes +enum AutoMode { + Auto_TakeOff, + Auto_WP, + Auto_Land, + Auto_RTL, + Auto_CircleMoveToEdge, + Auto_Circle, + Auto_Spline, + Auto_NavGuided, + Auto_Loiter, + Auto_LoiterToAlt, + Auto_NavPayloadPlace, +}; + +// Guided modes +enum GuidedMode { + Guided_TakeOff, + Guided_WP, + Guided_Velocity, + Guided_PosVel, + Guided_Angle, +}; + +// // Airmode +// enum class AirMode { +// AIRMODE_NONE, +// AIRMODE_DISABLED, +// AIRMODE_ENABLED, +// }; + +// // Safe RTL states +// enum SmartRTLState { +// SmartRTL_WaitForPathCleanup, +// SmartRTL_PathFollow, +// SmartRTL_PreLandPosition, +// SmartRTL_Descend, +// SmartRTL_Land +// }; + +enum PayloadPlaceStateType { + PayloadPlaceStateType_FlyToLocation, + PayloadPlaceStateType_Calibrating_Hover_Start, + PayloadPlaceStateType_Calibrating_Hover, + PayloadPlaceStateType_Descending_Start, + PayloadPlaceStateType_Descending, + PayloadPlaceStateType_Releasing_Start, + PayloadPlaceStateType_Releasing, + PayloadPlaceStateType_Released, + PayloadPlaceStateType_Ascending_Start, + PayloadPlaceStateType_Ascending, + PayloadPlaceStateType_Done, +}; + +// bit options for DEV_OPTIONS parameter +enum DevOptions { + DevOptionADSBMAVLink = 1, + DevOptionVFR_HUDRelativeAlt = 2, + DevOptionSetAttitudeTarget_ThrustAsThrust = 4, +}; + +// Logging parameters +enum LoggingParameters { + LOG_CONTROL_TUNING_MSG, + LOG_DATA_INT16_MSG, + LOG_DATA_UINT16_MSG, + LOG_DATA_INT32_MSG, + LOG_DATA_UINT32_MSG, + LOG_DATA_FLOAT_MSG, + LOG_MOTBATT_MSG, + LOG_PARAMTUNE_MSG, + LOG_HELI_MSG, + LOG_PRECLAND_MSG, + LOG_GUIDEDTARGET_MSG, + LOG_SYSIDD_MSG, + LOG_SYSIDS_MSG, +}; + +#define MASK_LOG_ATTITUDE_FAST (1<<0) +#define MASK_LOG_ATTITUDE_MED (1<<1) +#define MASK_LOG_GPS (1<<2) +#define MASK_LOG_PM (1<<3) +#define MASK_LOG_CTUN (1<<4) +#define MASK_LOG_NTUN (1<<5) +#define MASK_LOG_RCIN (1<<6) +#define MASK_LOG_IMU (1<<7) +#define MASK_LOG_CMD (1<<8) +#define MASK_LOG_CURRENT (1<<9) +#define MASK_LOG_RCOUT (1<<10) +#define MASK_LOG_OPTFLOW (1<<11) +#define MASK_LOG_PID (1<<12) +#define MASK_LOG_COMPASS (1<<13) +#define MASK_LOG_INAV (1<<14) // deprecated +#define MASK_LOG_CAMERA (1<<15) +#define MASK_LOG_MOTBATT (1UL<<17) +#define MASK_LOG_IMU_FAST (1UL<<18) +#define MASK_LOG_IMU_RAW (1UL<<19) +#define MASK_LOG_ANY 0xFFFF + +// Radio failsafe definitions (FS_THR parameter) +#define FS_THR_DISABLED 0 +#define FS_THR_ENABLED_ALWAYS_RTL 1 +#define FS_THR_ENABLED_CONTINUE_MISSION 2 // Removed in 4.0+, now use fs_options +#define FS_THR_ENABLED_ALWAYS_LAND 3 +#define FS_THR_ENABLED_ALWAYS_SMARTRTL_OR_RTL 4 +#define FS_THR_ENABLED_ALWAYS_SMARTRTL_OR_LAND 5 + +// GCS failsafe definitions (FS_GCS_ENABLE parameter) +#define FS_GCS_DISABLED 0 +#define FS_GCS_ENABLED_ALWAYS_RTL 1 +#define FS_GCS_ENABLED_CONTINUE_MISSION 2 // Removed in 4.0+, now use fs_options +#define FS_GCS_ENABLED_ALWAYS_SMARTRTL_OR_RTL 3 +#define FS_GCS_ENABLED_ALWAYS_SMARTRTL_OR_LAND 4 +#define FS_GCS_ENABLED_ALWAYS_LAND 5 + +// EKF failsafe definitions (FS_EKF_ACTION parameter) +#define FS_EKF_ACTION_LAND 1 // switch to LAND mode on EKF failsafe +#define FS_EKF_ACTION_ALTHOLD 2 // switch to ALTHOLD mode on EKF failsafe +#define FS_EKF_ACTION_LAND_EVEN_STABILIZE 3 // switch to Land mode on EKF failsafe even if in a manual flight mode like stabilize + +// for PILOT_THR_BHV parameter +#define THR_BEHAVE_FEEDBACK_FROM_MID_STICK (1<<0) +#define THR_BEHAVE_HIGH_THROTTLE_CANCELS_LAND (1<<1) +#define THR_BEHAVE_DISARM_ON_LAND_DETECT (1<<2) diff --git a/Blimp/ekf_check.cpp b/Blimp/ekf_check.cpp new file mode 100644 index 0000000000..11ec4e1178 --- /dev/null +++ b/Blimp/ekf_check.cpp @@ -0,0 +1,272 @@ +#include "Blimp.h" + +/** + * + * Detects failures of the ekf or inertial nav system triggers an alert + * to the pilot and helps take countermeasures + * + */ + +#ifndef EKF_CHECK_ITERATIONS_MAX +# define EKF_CHECK_ITERATIONS_MAX 10 // 1 second (ie. 10 iterations at 10hz) of bad variances signals a failure +#endif + +#ifndef EKF_CHECK_WARNING_TIME +# define EKF_CHECK_WARNING_TIME (30*1000) // warning text messages are sent to ground no more than every 30 seconds +#endif + +//////////////////////////////////////////////////////////////////////////////// +// EKF_check structure +//////////////////////////////////////////////////////////////////////////////// +static struct { + uint8_t fail_count; // number of iterations ekf or dcm have been out of tolerances + uint8_t bad_variance : 1; // true if ekf should be considered untrusted (fail_count has exceeded EKF_CHECK_ITERATIONS_MAX) + uint32_t last_warn_time; // system time of last warning in milliseconds. Used to throttle text warnings sent to GCS +} ekf_check_state; + +// ekf_check - detects if ekf variance are out of tolerance and triggers failsafe +// should be called at 10hz +void Blimp::ekf_check() +{ + // ensure EKF_CHECK_ITERATIONS_MAX is at least 7 + static_assert(EKF_CHECK_ITERATIONS_MAX >= 7, "EKF_CHECK_ITERATIONS_MAX must be at least 7"); + + // exit immediately if ekf has no origin yet - this assumes the origin can never become unset + Location temp_loc; + if (!ahrs.get_origin(temp_loc)) { + return; + } + + // return immediately if motors are not armed, or ekf check is disabled + if (!motors->armed() || (g.fs_ekf_thresh <= 0.0f)) { + ekf_check_state.fail_count = 0; + ekf_check_state.bad_variance = false; + AP_Notify::flags.ekf_bad = ekf_check_state.bad_variance; + failsafe_ekf_off_event(); // clear failsafe + return; + } + + // compare compass and velocity variance vs threshold and also check + // if we are still navigating + bool is_navigating = ekf_has_relative_position() || ekf_has_absolute_position(); + if (ekf_over_threshold() || !is_navigating) { + // if compass is not yet flagged as bad + if (!ekf_check_state.bad_variance) { + // increase counter + ekf_check_state.fail_count++; + if (ekf_check_state.fail_count == (EKF_CHECK_ITERATIONS_MAX-2) && ekf_over_threshold()) { + // we are two iterations away from declaring an EKF failsafe, ask the EKF if we can reset + // yaw to resolve the issue + ahrs.request_yaw_reset(); + } + if (ekf_check_state.fail_count == (EKF_CHECK_ITERATIONS_MAX-1)) { + // we are just about to declare a EKF failsafe, ask the EKF if we can + // change lanes to resolve the issue + ahrs.check_lane_switch(); + } + // if counter above max then trigger failsafe + if (ekf_check_state.fail_count >= EKF_CHECK_ITERATIONS_MAX) { + // limit count from climbing too high + ekf_check_state.fail_count = EKF_CHECK_ITERATIONS_MAX; + ekf_check_state.bad_variance = true; + AP::logger().Write_Error(LogErrorSubsystem::EKFCHECK, LogErrorCode::EKFCHECK_BAD_VARIANCE); + // send message to gcs + if ((AP_HAL::millis() - ekf_check_state.last_warn_time) > EKF_CHECK_WARNING_TIME) { + gcs().send_text(MAV_SEVERITY_CRITICAL,"EKF variance"); + ekf_check_state.last_warn_time = AP_HAL::millis(); + } + failsafe_ekf_event(); + } + } + } else { + // reduce counter + if (ekf_check_state.fail_count > 0) { + ekf_check_state.fail_count--; + + // if compass is flagged as bad and the counter reaches zero then clear flag + if (ekf_check_state.bad_variance && ekf_check_state.fail_count == 0) { + ekf_check_state.bad_variance = false; + AP::logger().Write_Error(LogErrorSubsystem::EKFCHECK, LogErrorCode::EKFCHECK_VARIANCE_CLEARED); + // clear failsafe + failsafe_ekf_off_event(); + } + } + } + + // set AP_Notify flags + AP_Notify::flags.ekf_bad = ekf_check_state.bad_variance; + + // To-Do: add ekf variances to extended status +} + +// ekf_over_threshold - returns true if the ekf's variance are over the tolerance +bool Blimp::ekf_over_threshold() +{ + // return false immediately if disabled + if (g.fs_ekf_thresh <= 0.0f) { + return false; + } + + // use EKF to get variance + float position_variance, vel_variance, height_variance, tas_variance; + Vector3f mag_variance; + ahrs.get_variances(vel_variance, position_variance, height_variance, mag_variance, tas_variance); + + const float mag_max = fmaxf(fmaxf(mag_variance.x,mag_variance.y),mag_variance.z); + + // return true if two of compass, velocity and position variances are over the threshold OR velocity variance is twice the threshold + uint8_t over_thresh_count = 0; + if (mag_max >= g.fs_ekf_thresh) { + over_thresh_count++; + } + + bool optflow_healthy = false; + if (!optflow_healthy && (vel_variance >= (2.0f * g.fs_ekf_thresh))) { + over_thresh_count += 2; + } else if (vel_variance >= g.fs_ekf_thresh) { + over_thresh_count++; + } + + if ((position_variance >= g.fs_ekf_thresh && over_thresh_count >= 1) || over_thresh_count >= 2) { + return true; + } + + return false; +} + + +// failsafe_ekf_event - perform ekf failsafe +void Blimp::failsafe_ekf_event() +{ + // return immediately if ekf failsafe already triggered + if (failsafe.ekf) { + return; + } + + // EKF failsafe event has occurred + failsafe.ekf = true; + AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_EKFINAV, LogErrorCode::FAILSAFE_OCCURRED); + + // // sometimes LAND *does* require GPS so ensure we are in non-GPS land + // if (control_mode == Mode::Number::LAND && landing_with_GPS()) { + // mode_land.do_not_use_GPS(); + // return; + // } + // LAND never requires GPS. + + // does this mode require position? + if (!blimp.flightmode->requires_GPS() && (g.fs_ekf_action != FS_EKF_ACTION_LAND_EVEN_STABILIZE)) { + return; + } + + // take action based on fs_ekf_action parameter + switch (g.fs_ekf_action) { + case FS_EKF_ACTION_LAND: + case FS_EKF_ACTION_LAND_EVEN_STABILIZE: + default: + set_mode_land_with_pause(ModeReason::EKF_FAILSAFE); + break; + } +} + +// failsafe_ekf_off_event - actions to take when EKF failsafe is cleared +void Blimp::failsafe_ekf_off_event(void) +{ + // return immediately if not in ekf failsafe + if (!failsafe.ekf) { + return; + } + + failsafe.ekf = false; + AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_EKFINAV, LogErrorCode::FAILSAFE_RESOLVED); +} + +// check for ekf yaw reset and adjust target heading, also log position reset +void Blimp::check_ekf_reset() +{ + // check for yaw reset + float yaw_angle_change_rad; + uint32_t new_ekfYawReset_ms = ahrs.getLastYawResetAngle(yaw_angle_change_rad); + if (new_ekfYawReset_ms != ekfYawReset_ms) { + // attitude_control->inertial_frame_reset(); + ekfYawReset_ms = new_ekfYawReset_ms; + AP::logger().Write_Event(LogEvent::EKF_YAW_RESET); + } + +#if AP_AHRS_NAVEKF_AVAILABLE && (HAL_NAVEKF2_AVAILABLE || HAL_NAVEKF3_AVAILABLE) + // check for change in primary EKF, reset attitude target and log. AC_PosControl handles position target adjustment + if ((ahrs.get_primary_core_index() != ekf_primary_core) && (ahrs.get_primary_core_index() != -1)) { + // attitude_control->inertial_frame_reset(); + ekf_primary_core = ahrs.get_primary_core_index(); + AP::logger().Write_Error(LogErrorSubsystem::EKF_PRIMARY, LogErrorCode(ekf_primary_core)); + gcs().send_text(MAV_SEVERITY_WARNING, "EKF primary changed:%d", (unsigned)ekf_primary_core); + } +#endif +} + +// check for high vibrations affecting altitude control +void Blimp::check_vibration() +{ + uint32_t now = AP_HAL::millis(); + + // assume checks will succeed + bool checks_succeeded = true; + + // check if vertical velocity and position innovations are positive (NKF3.IVD & NKF3.IPD are both positive) + Vector3f vel_innovation; + Vector3f pos_innovation; + Vector3f mag_innovation; + float tas_innovation; + float yaw_innovation; + if (!ahrs.get_innovations(vel_innovation, pos_innovation, mag_innovation, tas_innovation, yaw_innovation)) { + checks_succeeded = false; + } + const bool innov_velD_posD_positive = is_positive(vel_innovation.z) && is_positive(pos_innovation.z); + + // check if vertical velocity variance is at least 1 (NK4.SV >= 1.0) + float position_variance, vel_variance, height_variance, tas_variance; + Vector3f mag_variance; + if (!ahrs.get_variances(vel_variance, position_variance, height_variance, mag_variance, tas_variance)) { + checks_succeeded = false; + } + + // if no failure + if ((g2.fs_vibe_enabled == 0) || !checks_succeeded || !motors->armed() || !innov_velD_posD_positive || (vel_variance < 1.0f)) { + if (vibration_check.high_vibes) { + // start clear time + if (vibration_check.clear_ms == 0) { + vibration_check.clear_ms = now; + return; + } + // turn off vibration compensation after 15 seconds + if (now - vibration_check.clear_ms > 15000) { + // restore ekf gains, reset timers and update user + vibration_check.high_vibes = false; + // pos_control->set_vibe_comp(false); + vibration_check.clear_ms = 0; + AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_VIBE, LogErrorCode::FAILSAFE_RESOLVED); + gcs().send_text(MAV_SEVERITY_CRITICAL, "Vibration compensation OFF"); + } + } + vibration_check.start_ms = 0; + return; + } + + // start timer + if (vibration_check.start_ms == 0) { + vibration_check.start_ms = now; + vibration_check.clear_ms = 0; + return; + } + + // check if failure has persisted for at least 1 second + if (now - vibration_check.start_ms > 1000) { + if (!vibration_check.high_vibes) { + // switch ekf to use resistant gains + vibration_check.high_vibes = true; + // pos_control->set_vibe_comp(true); + AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_VIBE, LogErrorCode::FAILSAFE_OCCURRED); + gcs().send_text(MAV_SEVERITY_CRITICAL, "Vibration compensation ON"); + } + } +} diff --git a/Blimp/events.cpp b/Blimp/events.cpp new file mode 100644 index 0000000000..44f5924132 --- /dev/null +++ b/Blimp/events.cpp @@ -0,0 +1,343 @@ +#include "Blimp.h" + +/* + * This event will be called when the failsafe changes + * boolean failsafe reflects the current state + */ + + +bool Blimp::failsafe_option(FailsafeOption opt) const +{ + return (g2.fs_options & (uint32_t)opt); +} + +void Blimp::failsafe_radio_on_event() +{ + AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_RADIO, LogErrorCode::FAILSAFE_OCCURRED); + + // set desired action based on FS_THR_ENABLE parameter + Failsafe_Action desired_action; + switch (g.failsafe_throttle) { + case FS_THR_DISABLED: + desired_action = Failsafe_Action_None; + break; + case FS_THR_ENABLED_ALWAYS_LAND: + desired_action = Failsafe_Action_Land; + break; + default: + desired_action = Failsafe_Action_Land; + } + + // Conditions to deviate from FS_THR_ENABLE selection and send specific GCS warning + if (should_disarm_on_failsafe()) { + // should immediately disarm when we're on the ground + gcs().send_text(MAV_SEVERITY_WARNING, "Radio Failsafe - Disarming"); + arming.disarm(AP_Arming::Method::RADIOFAILSAFE); + desired_action = Failsafe_Action_None; + + } else if (flightmode->is_landing() && ((battery.has_failsafed() && battery.get_highest_failsafe_priority() <= FAILSAFE_LAND_PRIORITY))) { + // Allow landing to continue when battery failsafe requires it (not a user option) + gcs().send_text(MAV_SEVERITY_WARNING, "Radio + Battery Failsafe - Continuing Landing"); + desired_action = Failsafe_Action_Land; + + } else if (flightmode->is_landing() && failsafe_option(FailsafeOption::CONTINUE_IF_LANDING)) { + // Allow landing to continue when FS_OPTIONS is set to continue landing + gcs().send_text(MAV_SEVERITY_WARNING, "Radio Failsafe - Continuing Landing"); + desired_action = Failsafe_Action_Land; + + } else { + gcs().send_text(MAV_SEVERITY_WARNING, "Radio Failsafe"); + } + + // Call the failsafe action handler + do_failsafe_action(desired_action, ModeReason::RADIO_FAILSAFE); +} + +// failsafe_off_event - respond to radio contact being regained +void Blimp::failsafe_radio_off_event() +{ + // no need to do anything except log the error as resolved + // user can now override roll, pitch, yaw and throttle and even use flight mode switch to restore previous flight mode + AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_RADIO, LogErrorCode::FAILSAFE_RESOLVED); + gcs().send_text(MAV_SEVERITY_WARNING, "Radio Failsafe Cleared"); +} + +void Blimp::handle_battery_failsafe(const char *type_str, const int8_t action) +{ + AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_BATT, LogErrorCode::FAILSAFE_OCCURRED); + + Failsafe_Action desired_action = (Failsafe_Action)action; + + // Conditions to deviate from BATT_FS_XXX_ACT parameter setting + if (should_disarm_on_failsafe()) { + // should immediately disarm when we're on the ground + arming.disarm(AP_Arming::Method::BATTERYFAILSAFE); + desired_action = Failsafe_Action_None; + gcs().send_text(MAV_SEVERITY_WARNING, "Battery Failsafe - Disarming"); + + } else if (flightmode->is_landing() && failsafe_option(FailsafeOption::CONTINUE_IF_LANDING) && desired_action != Failsafe_Action_None) { + // Allow landing to continue when FS_OPTIONS is set to continue when landing + desired_action = Failsafe_Action_Land; + gcs().send_text(MAV_SEVERITY_WARNING, "Battery Failsafe - Continuing Landing"); + } else { + gcs().send_text(MAV_SEVERITY_WARNING, "Battery Failsafe"); + } + + // Battery FS options already use the Failsafe_Options enum. So use them directly. + do_failsafe_action(desired_action, ModeReason::BATTERY_FAILSAFE); + +} +// failsafe_gcs_check - check for ground station failsafe +void Blimp::failsafe_gcs_check() +{ + // Bypass GCS failsafe checks if disabled or GCS never connected + if (g.failsafe_gcs == FS_GCS_DISABLED || failsafe.last_heartbeat_ms == 0) { + return; + } + + // calc time since last gcs update + // note: this only looks at the heartbeat from the device id set by g.sysid_my_gcs + const uint32_t last_gcs_update_ms = millis() - failsafe.last_heartbeat_ms; + const uint32_t gcs_timeout_ms = uint32_t(constrain_float(g2.fs_gcs_timeout * 1000.0f, 0.0f, UINT32_MAX)); + + // Determine which event to trigger + if (last_gcs_update_ms < gcs_timeout_ms && failsafe.gcs) { + // Recovery from a GCS failsafe + set_failsafe_gcs(false); + // failsafe_gcs_off_event(); + + } else if (last_gcs_update_ms < gcs_timeout_ms && !failsafe.gcs) { + // No problem, do nothing + + } else if (last_gcs_update_ms > gcs_timeout_ms && failsafe.gcs) { + // Already in failsafe, do nothing + + } else if (last_gcs_update_ms > gcs_timeout_ms && !failsafe.gcs) { + // New GCS failsafe event, trigger events + set_failsafe_gcs(true); + // failsafe_gcs_on_event(); + } +} +/* +// failsafe_gcs_on_event - actions to take when GCS contact is lost +void Blimp::failsafe_gcs_on_event(void) +{ + AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_GCS, LogErrorCode::FAILSAFE_OCCURRED); + RC_Channels::clear_overrides(); + + // convert the desired failsafe response to the Failsafe_Action enum + Failsafe_Action desired_action; + switch (g.failsafe_gcs) { + case FS_GCS_DISABLED: + desired_action = Failsafe_Action_None; + break; + case FS_GCS_ENABLED_ALWAYS_RTL: + case FS_GCS_ENABLED_CONTINUE_MISSION: + desired_action = Failsafe_Action_RTL; + break; + case FS_GCS_ENABLED_ALWAYS_SMARTRTL_OR_RTL: + desired_action = Failsafe_Action_SmartRTL; + break; + case FS_GCS_ENABLED_ALWAYS_SMARTRTL_OR_LAND: + desired_action = Failsafe_Action_SmartRTL_Land; + break; + case FS_GCS_ENABLED_ALWAYS_LAND: + desired_action = Failsafe_Action_Land; + break; + default: // if an invalid parameter value is set, the fallback is RTL + desired_action = Failsafe_Action_RTL; + } + + // Conditions to deviate from FS_GCS_ENABLE parameter setting + if (!motors->armed()) { + desired_action = Failsafe_Action_None; + gcs().send_text(MAV_SEVERITY_WARNING, "GCS Failsafe"); + + } else if (should_disarm_on_failsafe()) { + // should immediately disarm when we're on the ground + arming.disarm(AP_Arming::Method::GCSFAILSAFE); + desired_action = Failsafe_Action_None; + gcs().send_text(MAV_SEVERITY_WARNING, "GCS Failsafe - Disarming"); + + } else if (flightmode->is_landing() && ((battery.has_failsafed() && battery.get_highest_failsafe_priority() <= FAILSAFE_LAND_PRIORITY))) { + // Allow landing to continue when battery failsafe requires it (not a user option) + gcs().send_text(MAV_SEVERITY_WARNING, "GCS + Battery Failsafe - Continuing Landing"); + desired_action = Failsafe_Action_Land; + + } else if (flightmode->is_landing() && failsafe_option(FailsafeOption::CONTINUE_IF_LANDING)) { + // Allow landing to continue when FS_OPTIONS is set to continue landing + gcs().send_text(MAV_SEVERITY_WARNING, "GCS Failsafe - Continuing Landing"); + desired_action = Failsafe_Action_Land; + + } else if (control_mode == Mode::Number::AUTO && failsafe_option(FailsafeOption::GCS_CONTINUE_IF_AUTO)) { + // Allow mission to continue when FS_OPTIONS is set to continue mission + gcs().send_text(MAV_SEVERITY_WARNING, "GCS Failsafe - Continuing Auto Mode"); + desired_action = Failsafe_Action_None; + + } else if (failsafe_option(FailsafeOption::GCS_CONTINUE_IF_PILOT_CONTROL) && !flightmode->is_autopilot()) { + // should continue when in a pilot controlled mode because FS_OPTIONS is set to continue in pilot controlled modes + gcs().send_text(MAV_SEVERITY_WARNING, "GCS Failsafe - Continuing Pilot Control"); + desired_action = Failsafe_Action_None; + } else { + gcs().send_text(MAV_SEVERITY_WARNING, "GCS Failsafe"); + } + + // Call the failsafe action handler + do_failsafe_action(desired_action, ModeReason::GCS_FAILSAFE); +} + +// failsafe_gcs_off_event - actions to take when GCS contact is restored +void Blimp::failsafe_gcs_off_event(void) +{ + gcs().send_text(MAV_SEVERITY_WARNING, "GCS Failsafe Cleared"); + AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_GCS, LogErrorCode::FAILSAFE_RESOLVED); +} + +// executes terrain failsafe if data is missing for longer than a few seconds +void Blimp::failsafe_terrain_check() +{ + // trigger within milliseconds of failures while in various modes + bool timeout = (failsafe.terrain_last_failure_ms - failsafe.terrain_first_failure_ms) > FS_TERRAIN_TIMEOUT_MS; + bool trigger_event = timeout && flightmode->requires_terrain_failsafe(); + + // check for clearing of event + if (trigger_event != failsafe.terrain) { + if (trigger_event) { + failsafe_terrain_on_event(); + } else { + AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_TERRAIN, LogErrorCode::ERROR_RESOLVED); + failsafe.terrain = false; + } + } +} + +// set terrain data status (found or not found) +void Blimp::failsafe_terrain_set_status(bool data_ok) +{ + uint32_t now = millis(); + + // record time of first and latest failures (i.e. duration of failures) + if (!data_ok) { + failsafe.terrain_last_failure_ms = now; + if (failsafe.terrain_first_failure_ms == 0) { + failsafe.terrain_first_failure_ms = now; + } + } else { + // failures cleared after 0.1 seconds of persistent successes + if (now - failsafe.terrain_last_failure_ms > 100) { + failsafe.terrain_last_failure_ms = 0; + failsafe.terrain_first_failure_ms = 0; + } + } +} + +// terrain failsafe action +void Blimp::failsafe_terrain_on_event() +{ + failsafe.terrain = true; + gcs().send_text(MAV_SEVERITY_CRITICAL,"Failsafe: Terrain data missing"); + AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_TERRAIN, LogErrorCode::FAILSAFE_OCCURRED); + + if (should_disarm_on_failsafe()) { + arming.disarm(AP_Arming::Method::TERRAINFAILSAFE); +#if MODE_RTL_ENABLED == ENABLED + } else if (control_mode == Mode::Number::RTL) { + mode_rtl.restart_without_terrain(); +#endif + } else { + set_mode_RTL_or_land_with_pause(ModeReason::TERRAIN_FAILSAFE); + } +} + +// check for gps glitch failsafe +void Blimp::gpsglitch_check() +{ + // get filter status + nav_filter_status filt_status = inertial_nav.get_filter_status(); + bool gps_glitching = filt_status.flags.gps_glitching; + + // log start or stop of gps glitch. AP_Notify update is handled from within AP_AHRS + if (ap.gps_glitching != gps_glitching) { + ap.gps_glitching = gps_glitching; + if (gps_glitching) { + AP::logger().Write_Error(LogErrorSubsystem::GPS, LogErrorCode::GPS_GLITCH); + gcs().send_text(MAV_SEVERITY_CRITICAL,"GPS Glitch"); + } else { + AP::logger().Write_Error(LogErrorSubsystem::GPS, LogErrorCode::ERROR_RESOLVED); + gcs().send_text(MAV_SEVERITY_CRITICAL,"GPS Glitch cleared"); + } + } +} + +// set_mode_RTL_or_land_with_pause - sets mode to RTL if possible or LAND with 4 second delay before descent starts +// this is always called from a failsafe so we trigger notification to pilot +void Blimp::set_mode_RTL_or_land_with_pause(ModeReason reason) +{ + // attempt to switch to RTL, if this fails then switch to Land + if (!set_mode(Mode::Number::RTL, reason)) { + // set mode to land will trigger mode change notification to pilot + set_mode_land_with_pause(reason); + } else { + // alert pilot to mode change + AP_Notify::events.failsafe_mode_change = 1; + } +} + +// set_mode_SmartRTL_or_land_with_pause - sets mode to SMART_RTL if possible or LAND with 4 second delay before descent starts +// this is always called from a failsafe so we trigger notification to pilot +void Blimp::set_mode_SmartRTL_or_land_with_pause(ModeReason reason) +{ + // attempt to switch to SMART_RTL, if this failed then switch to Land + if (!set_mode(Mode::Number::SMART_RTL, reason)) { + gcs().send_text(MAV_SEVERITY_WARNING, "SmartRTL Unavailable, Using Land Mode"); + set_mode_land_with_pause(reason); + } else { + AP_Notify::events.failsafe_mode_change = 1; + } +} + +// set_mode_SmartRTL_or_RTL - sets mode to SMART_RTL if possible or RTL if possible or LAND with 4 second delay before descent starts +// this is always called from a failsafe so we trigger notification to pilot +void Blimp::set_mode_SmartRTL_or_RTL(ModeReason reason) +{ + // attempt to switch to SmartRTL, if this failed then attempt to RTL + // if that fails, then land + if (!set_mode(Mode::Number::SMART_RTL, reason)) { + gcs().send_text(MAV_SEVERITY_WARNING, "SmartRTL Unavailable, Trying RTL Mode"); + set_mode_RTL_or_land_with_pause(reason); + } else { + AP_Notify::events.failsafe_mode_change = 1; + } +} +*/ +bool Blimp::should_disarm_on_failsafe() +{ + if (ap.in_arming_delay) { + return true; + } + + switch (control_mode) { + case Mode::Number::MANUAL: + default: + // if landed disarm + return ap.land_complete; + } +} + + +void Blimp::do_failsafe_action(Failsafe_Action action, ModeReason reason) +{ + + // Execute the specified desired_action + switch (action) { + case Failsafe_Action_None: + return; + case Failsafe_Action_Land: + set_mode_land_with_pause(reason); + break; + case Failsafe_Action_Terminate: { + arming.disarm(AP_Arming::Method::FAILSAFE_ACTION_TERMINATE); + } + break; + } +} diff --git a/Blimp/failsafe.cpp b/Blimp/failsafe.cpp new file mode 100644 index 0000000000..adf9fc67dc --- /dev/null +++ b/Blimp/failsafe.cpp @@ -0,0 +1,72 @@ +#include "Blimp.h" + +// +// failsafe support +// Andrew Tridgell, December 2011 +// +// our failsafe strategy is to detect main loop lockup and disarm the motors +// + +static bool failsafe_enabled = false; +static uint16_t failsafe_last_ticks; +static uint32_t failsafe_last_timestamp; +static bool in_failsafe; + +// +// failsafe_enable - enable failsafe +// +void Blimp::failsafe_enable() +{ + failsafe_enabled = true; + failsafe_last_timestamp = micros(); +} + +// +// failsafe_disable - used when we know we are going to delay the mainloop significantly +// +void Blimp::failsafe_disable() +{ + failsafe_enabled = false; +} + +// +// failsafe_check - this function is called from the core timer interrupt at 1kHz. +// +void Blimp::failsafe_check() +{ + uint32_t tnow = AP_HAL::micros(); + + const uint16_t ticks = scheduler.ticks(); + if (ticks != failsafe_last_ticks) { + // the main loop is running, all is OK + failsafe_last_ticks = ticks; + failsafe_last_timestamp = tnow; + if (in_failsafe) { + in_failsafe = false; + AP::logger().Write_Error(LogErrorSubsystem::CPU, LogErrorCode::FAILSAFE_RESOLVED); + } + return; + } + + if (!in_failsafe && failsafe_enabled && tnow - failsafe_last_timestamp > 2000000) { + // motors are running but we have gone 2 second since the + // main loop ran. That means we're in trouble and should + // disarm the motors-> + in_failsafe = true; + // reduce motors to minimum (we do not immediately disarm because we want to log the failure) + if (motors->armed()) { + motors->output_min(); + } + + AP::logger().Write_Error(LogErrorSubsystem::CPU, LogErrorCode::FAILSAFE_OCCURRED); + } + + if (failsafe_enabled && in_failsafe && tnow - failsafe_last_timestamp > 1000000) { + // disarm motors every second + failsafe_last_timestamp = tnow; + if (motors->armed()) { + motors->armed(false); + motors->output(); + } + } +} diff --git a/Blimp/inertia.cpp b/Blimp/inertia.cpp new file mode 100644 index 0000000000..874612a69e --- /dev/null +++ b/Blimp/inertia.cpp @@ -0,0 +1,27 @@ +#include "Blimp.h" + +// read_inertia - read inertia in from accelerometers +void Blimp::read_inertia() +{ + // inertial altitude estimates. Use barometer climb rate during high vibrations + inertial_nav.update(vibration_check.high_vibes); + + // pull position from ahrs + Location loc; + ahrs.get_position(loc); + current_loc.lat = loc.lat; + current_loc.lng = loc.lng; + + // exit immediately if we do not have an altitude estimate + if (!inertial_nav.get_filter_status().flags.vert_pos) { + return; + } + + // current_loc.alt is alt-above-home, converted from inertial nav's alt-above-ekf-origin + const int32_t alt_above_origin_cm = inertial_nav.get_altitude(); + current_loc.set_alt_cm(alt_above_origin_cm, Location::AltFrame::ABOVE_ORIGIN); + if (!ahrs.home_is_set() || !current_loc.change_alt_frame(Location::AltFrame::ABOVE_HOME)) { + // if home has not been set yet we treat alt-above-origin as alt-above-home + current_loc.set_alt_cm(alt_above_origin_cm, Location::AltFrame::ABOVE_HOME); + } +} diff --git a/Blimp/mode.cpp b/Blimp/mode.cpp new file mode 100644 index 0000000000..9c9239d7db --- /dev/null +++ b/Blimp/mode.cpp @@ -0,0 +1,537 @@ +#include "Blimp.h" + +/* + * High level calls to set and update flight modes logic for individual + * flight modes is in control_acro.cpp, control_stabilize.cpp, etc + */ + +/* + constructor for Mode object + */ +Mode::Mode(void) : + g(blimp.g), + g2(blimp.g2), + // wp_nav(blimp.wp_nav), + // loiter_nav(blimp.loiter_nav), + // pos_control(blimp.pos_control), + inertial_nav(blimp.inertial_nav), + ahrs(blimp.ahrs), + // attitude_control(blimp.attitude_control), + motors(blimp.motors), + channel_right(blimp.channel_right), + channel_front(blimp.channel_front), + channel_down(blimp.channel_down), + channel_yaw(blimp.channel_yaw), + G_Dt(blimp.G_Dt) +{ }; + +// return the static controller object corresponding to supplied mode +Mode *Blimp::mode_from_mode_num(const Mode::Number mode) +{ + Mode *ret = nullptr; + + switch (mode) { + case Mode::Number::MANUAL: + ret = &mode_manual; + break; + case Mode::Number::LAND: + ret = &mode_land; + break; + default: + break; + } + + return ret; +} + + +// set_mode - change flight mode and perform any necessary initialisation +// optional force parameter used to force the flight mode change (used only first time mode is set) +// returns true if mode was successfully set +// ACRO, STABILIZE, ALTHOLD, LAND, DRIFT and SPORT can always be set successfully but the return state of other flight modes should be checked and the caller should deal with failures appropriately +bool Blimp::set_mode(Mode::Number mode, ModeReason reason) +{ + + // return immediately if we are already in the desired mode + if (mode == control_mode) { + control_mode_reason = reason; + return true; + } + + Mode *new_flightmode = mode_from_mode_num((Mode::Number)mode); + if (new_flightmode == nullptr) { + gcs().send_text(MAV_SEVERITY_WARNING,"No such mode"); + AP::logger().Write_Error(LogErrorSubsystem::FLIGHT_MODE, LogErrorCode(mode)); + return false; + } + + bool ignore_checks = !motors->armed(); // allow switching to any mode if disarmed. We rely on the arming check to perform + + // ensure vehicle doesn't leap off the ground if a user switches + // into a manual throttle mode from a non-manual-throttle mode + // (e.g. user arms in guided, raises throttle to 1300 (not enough to + // trigger auto takeoff), then switches into manual): + bool user_throttle = new_flightmode->has_manual_throttle(); + if (!ignore_checks && + ap.land_complete && + user_throttle && + !blimp.flightmode->has_manual_throttle() && + new_flightmode->get_pilot_desired_throttle() > blimp.get_non_takeoff_throttle()) { + gcs().send_text(MAV_SEVERITY_WARNING, "Mode change failed: throttle too high"); + AP::logger().Write_Error(LogErrorSubsystem::FLIGHT_MODE, LogErrorCode(mode)); + return false; + } + + if (!ignore_checks && + new_flightmode->requires_GPS() && + !blimp.position_ok()) { + gcs().send_text(MAV_SEVERITY_WARNING, "Mode change failed: %s requires position", new_flightmode->name()); + AP::logger().Write_Error(LogErrorSubsystem::FLIGHT_MODE, LogErrorCode(mode)); + return false; + } + + // check for valid altitude if old mode did not require it but new one does + // we only want to stop changing modes if it could make things worse + if (!ignore_checks && + !blimp.ekf_alt_ok() && + flightmode->has_manual_throttle() && + !new_flightmode->has_manual_throttle()) { + gcs().send_text(MAV_SEVERITY_WARNING, "Mode change failed: %s need alt estimate", new_flightmode->name()); + AP::logger().Write_Error(LogErrorSubsystem::FLIGHT_MODE, LogErrorCode(mode)); + return false; + } + + if (!new_flightmode->init(ignore_checks)) { + gcs().send_text(MAV_SEVERITY_WARNING,"Flight mode change failed %s", new_flightmode->name()); + AP::logger().Write_Error(LogErrorSubsystem::FLIGHT_MODE, LogErrorCode(mode)); + return false; + } + + // perform any cleanup required by previous flight mode + exit_mode(flightmode, new_flightmode); + + // store previous flight mode (only used by tradeheli's autorotation) + prev_control_mode = control_mode; + + // update flight mode + flightmode = new_flightmode; + control_mode = mode; + control_mode_reason = reason; + logger.Write_Mode((uint8_t)control_mode, reason); + gcs().send_message(MSG_HEARTBEAT); + + // update notify object + notify_flight_mode(); + + // return success + return true; +} + +bool Blimp::set_mode(const uint8_t new_mode, const ModeReason reason) +{ + static_assert(sizeof(Mode::Number) == sizeof(new_mode), "The new mode can't be mapped to the vehicles mode number"); +#ifdef DISALLOW_GCS_MODE_CHANGE_DURING_RC_FAILSAFE + if (reason == ModeReason::GCS_COMMAND && blimp.failsafe.radio) { + // don't allow mode changes while in radio failsafe + return false; + } +#endif + return blimp.set_mode(static_cast(new_mode), reason); +} + +// update_flight_mode - calls the appropriate attitude controllers based on flight mode +// called at 100hz or more +void Blimp::update_flight_mode() +{ + // surface_tracking.invalidate_for_logging(); // invalidate surface tracking alt, flight mode will set to true if used + + flightmode->run(); +} + +// exit_mode - high level call to organise cleanup as a flight mode is exited +void Blimp::exit_mode(Mode *&old_flightmode, + Mode *&new_flightmode) +{ + + // smooth throttle transition when switching from manual to automatic flight modes + if (old_flightmode->has_manual_throttle() && !new_flightmode->has_manual_throttle() && motors->armed() && !ap.land_complete) { + // this assumes all manual flight modes use get_pilot_desired_throttle to translate pilot input to output throttle + // set_accel_throttle_I_from_pilot_throttle(); + } + + // cancel any takeoffs in progress + // old_flightmode->takeoff_stop(); +} + +// notify_flight_mode - sets notify object based on current flight mode. Only used for OreoLED notify device +void Blimp::notify_flight_mode() +{ + AP_Notify::flags.autopilot_mode = flightmode->is_autopilot(); + AP_Notify::flags.flight_mode = (uint8_t)control_mode; + notify.set_flight_mode_str(flightmode->name4()); +} + +void Mode::update_navigation() +{ + // run autopilot to make high level decisions about control modes + run_autopilot(); +} + +// returns desired angle in centi-degrees +void Mode::get_pilot_desired_accelerations(float &right_out, float &front_out) const +{ + // throttle failsafe check + if (blimp.failsafe.radio || !blimp.ap.rc_receiver_present) { + right_out = 0; + front_out = 0; + return; + } + // fetch roll and pitch inputs + right_out = channel_right->get_control_in(); + front_out = channel_front->get_control_in(); + + + // // do circular limit + // float total_in = norm(pitch_out, roll_out); + // if (total_in > angle_limit) { + // float ratio = angle_limit / total_in; + // roll_out *= ratio; + // pitch_out *= ratio; + // } + + // do lateral tilt to euler roll conversion + // roll_out = (18000/M_PI) * atanf(cosf(pitch_out*(M_PI/18000))*tanf(roll_out*(M_PI/18000))); + + // roll_out and pitch_out are returned +} + +// bool Mode::_TakeOff::triggered(const float target_climb_rate) const +// { +// if (!blimp.ap.land_complete) { +// // can't take off if we're already flying +// return false; +// } +// if (target_climb_rate <= 0.0f) { +// // can't takeoff unless we want to go up... +// return false; +// } + +// if (blimp.motors->get_spool_state() != Fins::SpoolState::THROTTLE_UNLIMITED) { +// // hold aircraft on the ground until rotor speed runup has finished +// return false; +// } + +// return true; +// } + +bool Mode::is_disarmed_or_landed() const +{ + if (!motors->armed() || !blimp.ap.auto_armed || blimp.ap.land_complete) { + return true; + } + return false; +} + +void Mode::zero_throttle_and_relax_ac(bool spool_up) +{ + if (spool_up) { + motors->set_desired_spool_state(Fins::DesiredSpoolState::THROTTLE_UNLIMITED); + } else { + motors->set_desired_spool_state(Fins::DesiredSpoolState::SHUT_DOWN); + } +} + +// void Mode::zero_throttle_and_hold_attitude() +// { +// // run attitude controller +// attitude_control->input_rate_bf_roll_pitch_yaw(0.0f, 0.0f, 0.0f); +// attitude_control->set_throttle_out(0.0f, false, blimp.g.throttle_filt); +// } + +// void Mode::make_safe_spool_down() +// { +// // command aircraft to initiate the shutdown process +// motors->set_desired_spool_state(Fins::DesiredSpoolState::GROUND_IDLE); +// switch (motors->get_spool_state()) { + +// case Fins::SpoolState::SHUT_DOWN: +// case Fins::SpoolState::GROUND_IDLE: +// // relax controllers during idle states +// // attitude_control->reset_rate_controller_I_terms_smoothly(); +// // attitude_control->set_yaw_target_to_current_heading(); +// break; + +// case Fins::SpoolState::SPOOLING_UP: +// case Fins::SpoolState::THROTTLE_UNLIMITED: +// case Fins::SpoolState::SPOOLING_DOWN: +// // while transitioning though active states continue to operate normally +// break; +// } + +// // pos_control->relax_alt_hold_controllers(0.0f); // forces throttle output to go to zero +// // pos_control->update_z_controller(); +// // we may need to move this out +// // attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(0.0f, 0.0f, 0.0f); +// } + +/* + get a height above ground estimate for landing + */ +int32_t Mode::get_alt_above_ground_cm(void) +{ + int32_t alt_above_ground_cm; + // if (blimp.get_rangefinder_height_interpolated_cm(alt_above_ground_cm)) { + // return alt_above_ground_cm; + // } + // if (!pos_control->is_active_xy()) { + // return blimp.current_loc.alt; + // } + if (blimp.current_loc.get_alt_cm(Location::AltFrame::ABOVE_TERRAIN, alt_above_ground_cm)) { + return alt_above_ground_cm; + } + + // Assume the Earth is flat: + return blimp.current_loc.alt; +} + +// void Mode::land_run_vertical_control(bool pause_descent) +// { +// float cmb_rate = 0; +// if (!pause_descent) { +// float max_land_descent_velocity; +// if (g.land_speed_high > 0) { +// max_land_descent_velocity = -g.land_speed_high; +// } else { +// max_land_descent_velocity = pos_control->get_max_speed_down(); +// } + +// // Don't speed up for landing. +// max_land_descent_velocity = MIN(max_land_descent_velocity, -abs(g.land_speed)); + +// // Compute a vertical velocity demand such that the vehicle approaches g2.land_alt_low. Without the below constraint, this would cause the vehicle to hover at g2.land_alt_low. +// // cmb_rate = AC_AttitudeControl::sqrt_controller(MAX(g2.land_alt_low,100)-get_alt_above_ground_cm(), pos_control->get_pos_z_p().kP(), pos_control->get_max_accel_z(), G_Dt); + +// // Constrain the demanded vertical velocity so that it is between the configured maximum descent speed and the configured minimum descent speed. +// // cmb_rate = constrain_float(cmb_rate, max_land_descent_velocity, -abs(g.land_speed)); +// } + +// // update altitude target and call position controller +// pos_control->set_alt_target_from_climb_rate_ff(cmb_rate, G_Dt, true); +// pos_control->update_z_controller(); +// } + +// void Mode::land_run_horizontal_control() +// { +// float target_roll = 0.0f; +// float target_pitch = 0.0f; +// float target_yaw_rate = 0; + +// // relax loiter target if we might be landed +// if (blimp.ap.land_complete_maybe) { +// loiter_nav->soften_for_landing(); +// } + +// // process pilot inputs +// if (!blimp.failsafe.radio) { +// if ((g.throttle_behavior & THR_BEHAVE_HIGH_THROTTLE_CANCELS_LAND) != 0 && blimp.rc_throttle_control_in_filter.get() > LAND_CANCEL_TRIGGER_THR){ +// AP::logger().Write_Event(LogEvent::LAND_CANCELLED_BY_PILOT); +// // exit land if throttle is high +// if (!set_mode(Mode::Number::LOITER, ModeReason::THROTTLE_LAND_ESCAPE)) { +// set_mode(Mode::Number::ALT_HOLD, ModeReason::THROTTLE_LAND_ESCAPE); +// } +// } + +// if (g.land_repositioning) { + +// // convert pilot input to lean angles +// get_pilot_desired_lean_angles(target_roll, target_pitch, loiter_nav->get_angle_max_cd(), attitude_control->get_althold_lean_angle_max()); + +// // record if pilot has overridden roll or pitch +// if (!is_zero(target_roll) || !is_zero(target_pitch)) { +// if (!blimp.ap.land_repo_active) { +// AP::logger().Write_Event(LogEvent::LAND_REPO_ACTIVE); +// } +// blimp.ap.land_repo_active = true; +// } +// } + +// // get pilot's desired yaw rate +// target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in()); +// if (!is_zero(target_yaw_rate)) { +// auto_yaw.set_mode(AUTO_YAW_HOLD); +// } +// } + +// // process roll, pitch inputs +// loiter_nav->set_pilot_desired_acceleration(target_roll, target_pitch, G_Dt); + +// // run loiter controller +// loiter_nav->update(); + +// float nav_roll = loiter_nav->get_roll(); +// float nav_pitch = loiter_nav->get_pitch(); + +// if (g2.wp_navalt_min > 0) { +// // user has requested an altitude below which navigation +// // attitude is limited. This is used to prevent commanded roll +// // over on landing, which particularly affects heliblimps if +// // there is any position estimate drift after touchdown. We +// // limit attitude to 7 degrees below this limit and linearly +// // interpolate for 1m above that +// float attitude_limit_cd = linear_interpolate(700, blimp.aparm.angle_max, get_alt_above_ground_cm(), +// g2.wp_navalt_min*100U, (g2.wp_navalt_min+1)*100U); +// float total_angle_cd = norm(nav_roll, nav_pitch); +// if (total_angle_cd > attitude_limit_cd) { +// float ratio = attitude_limit_cd / total_angle_cd; +// nav_roll *= ratio; +// nav_pitch *= ratio; + +// // tell position controller we are applying an external limit +// pos_control->set_limit_accel_xy(); +// } +// } + +// // call attitude controller +// if (auto_yaw.mode() == AUTO_YAW_HOLD) { +// // roll & pitch from waypoint controller, yaw rate from pilot +// attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(nav_roll, nav_pitch, target_yaw_rate); +// } else { +// // roll, pitch from waypoint controller, yaw heading from auto_heading() +// attitude_control->input_euler_angle_roll_pitch_yaw(nav_roll, nav_pitch, auto_yaw.yaw(), true); +// } +// } + +float Mode::throttle_hover() const +{ + return motors->get_throttle_hover(); +} + +// transform pilot's manual throttle input to make hover throttle mid stick +// used only for manual throttle modes +// thr_mid should be in the range 0 to 1 +// returns throttle output 0 to 1 +float Mode::get_pilot_desired_throttle() const +{ + const float thr_mid = throttle_hover(); + int16_t throttle_control = channel_down->get_control_in(); + + int16_t mid_stick = blimp.get_throttle_mid(); + // protect against unlikely divide by zero + if (mid_stick <= 0) { + mid_stick = 500; + } + + // ensure reasonable throttle values + throttle_control = constrain_int16(throttle_control,0,1000); + + // calculate normalised throttle input + float throttle_in; + if (throttle_control < mid_stick) { + throttle_in = ((float)throttle_control)*0.5f/(float)mid_stick; + } else { + throttle_in = 0.5f + ((float)(throttle_control-mid_stick)) * 0.5f / (float)(1000-mid_stick); + } + + const float expo = constrain_float(-(thr_mid-0.5f)/0.375f, -0.5f, 1.0f); + // calculate the output throttle using the given expo function + float throttle_out = throttle_in*(1.0f-expo) + expo*throttle_in*throttle_in*throttle_in; + return throttle_out; +} + +// Mode::AltHoldModeState Mode::get_alt_hold_state(float target_climb_rate_cms) +// { +// // Alt Hold State Machine Determination +// if (!motors->armed()) { +// // the aircraft should moved to a shut down state +// motors->set_desired_spool_state(Fins::DesiredSpoolState::SHUT_DOWN); + +// // transition through states as aircraft spools down +// switch (motors->get_spool_state()) { + +// case Fins::SpoolState::SHUT_DOWN: +// return AltHold_MotorStopped; + +// case Fins::SpoolState::GROUND_IDLE: +// return AltHold_Landed_Ground_Idle; + +// default: +// return AltHold_Landed_Pre_Takeoff; +// } + +// } else if (takeoff.running() || takeoff.triggered(target_climb_rate_cms)) { +// // the aircraft is currently landed or taking off, asking for a positive climb rate and in THROTTLE_UNLIMITED +// // the aircraft should progress through the take off procedure +// return AltHold_Takeoff; + +// } else if (!blimp.ap.auto_armed || blimp.ap.land_complete) { +// // the aircraft is armed and landed +// if (target_climb_rate_cms < 0.0f && !blimp.ap.using_interlock) { +// // the aircraft should move to a ground idle state +// motors->set_desired_spool_state(Fins::DesiredSpoolState::GROUND_IDLE); + +// } else { +// // the aircraft should prepare for imminent take off +// motors->set_desired_spool_state(Fins::DesiredSpoolState::THROTTLE_UNLIMITED); +// } + +// if (motors->get_spool_state() == Fins::SpoolState::GROUND_IDLE) { +// // the aircraft is waiting in ground idle +// return AltHold_Landed_Ground_Idle; + +// } else { +// // the aircraft can leave the ground at any time +// return AltHold_Landed_Pre_Takeoff; +// } + +// } else { +// // the aircraft is in a flying state +// motors->set_desired_spool_state(Fins::DesiredSpoolState::THROTTLE_UNLIMITED); +// return AltHold_Flying; +// } +// } + +// pass-through functions to reduce code churn on conversion; +// these are candidates for moving into the Mode base +// class. +float Mode::get_pilot_desired_yaw_rate(int16_t stick_angle) +{ + return blimp.get_pilot_desired_yaw_rate(stick_angle); +} + +float Mode::get_pilot_desired_climb_rate(float throttle_control) +{ + return blimp.get_pilot_desired_climb_rate(throttle_control); +} + +float Mode::get_non_takeoff_throttle() +{ + return blimp.get_non_takeoff_throttle(); +} + +bool Mode::set_mode(Mode::Number mode, ModeReason reason) +{ + return blimp.set_mode(mode, reason); +} + +void Mode::set_land_complete(bool b) +{ + return blimp.set_land_complete(b); +} + +GCS_Blimp &Mode::gcs() +{ + return blimp.gcs(); +} + +// set_throttle_takeoff - allows modes to tell throttle controller we +// are taking off so I terms can be cleared +// void Mode::set_throttle_takeoff() +// { +// // tell position controller to reset alt target and reset I terms +// pos_control->init_takeoff(); +// } + +uint16_t Mode::get_pilot_speed_dn() +{ + return blimp.get_pilot_speed_dn(); +} diff --git a/Blimp/mode.h b/Blimp/mode.h new file mode 100644 index 0000000000..1c00fa39b8 --- /dev/null +++ b/Blimp/mode.h @@ -0,0 +1,374 @@ +#pragma once + +#include "Blimp.h" +class Parameters; +class ParametersG2; + +class GCS_Blimp; + +class Mode +{ + +public: + + // Auto Pilot Modes enumeration + enum class Number : uint8_t { + MANUAL = 0, // manual control similar to Copter's stabilize mode + LAND = 1, // currently just stops moving + // STABILIZE = 0, // manual airframe angle with manual throttle + // ACRO = 1, // manual body-frame angular rate with manual throttle + // ALT_HOLD = 2, // manual airframe angle with automatic throttle + // AUTO = 3, // fully automatic waypoint control using mission commands + // GUIDED = 4, // fully automatic fly to coordinate or fly at velocity/direction using GCS immediate commands + // LOITER = 5, // automatic horizontal acceleration with automatic throttle + // RTL = 6, // automatic return to launching point + // CIRCLE = 7, // automatic circular flight with automatic throttle + // LAND = 9, // automatic landing with horizontal position control + // DRIFT = 11, // semi-automous position, yaw and throttle control + // SPORT = 13, // manual earth-frame angular rate control with manual throttle + // FLIP = 14, // automatically flip the vehicle on the roll axis + // AUTOTUNE = 15, // automatically tune the vehicle's roll and pitch gains + // POSHOLD = 16, // automatic position hold with manual override, with automatic throttle + // BRAKE = 17, // full-brake using inertial/GPS system, no pilot input + // THROW = 18, // throw to launch mode using inertial/GPS system, no pilot input + // AVOID_ADSB = 19, // automatic avoidance of obstacles in the macro scale - e.g. full-sized aircraft + // GUIDED_NOGPS = 20, // guided mode but only accepts attitude and altitude + // SMART_RTL = 21, // SMART_RTL returns to home by retracing its steps + // FLOWHOLD = 22, // FLOWHOLD holds position with optical flow without rangefinder + // FOLLOW = 23, // follow attempts to follow another vehicle or ground station + // ZIGZAG = 24, // ZIGZAG mode is able to fly in a zigzag manner with predefined point A and point B + // SYSTEMID = 25, // System ID mode produces automated system identification signals in the controllers + // AUTOROTATE = 26, // Autonomous autorotation + }; + + // constructor + Mode(void); + + // do not allow copying + Mode(const Mode &other) = delete; + Mode &operator=(const Mode&) = delete; + + // child classes should override these methods + virtual bool init(bool ignore_checks) + { + return true; + } + virtual void run() = 0; + virtual bool requires_GPS() const = 0; + virtual bool has_manual_throttle() const = 0; + virtual bool allows_arming(bool from_gcs) const = 0; + virtual bool is_autopilot() const + { + return false; + } + virtual bool has_user_takeoff(bool must_navigate) const + { + return false; + } + virtual bool in_guided_mode() const + { + return false; + } + virtual bool logs_attitude() const + { + return false; + } + + // return a string for this flightmode + virtual const char *name() const = 0; + virtual const char *name4() const = 0; + + bool do_user_takeoff(float takeoff_alt_cm, bool must_navigate); + // virtual bool is_taking_off() const; + // static void takeoff_stop() { takeoff.stop(); } + + virtual bool is_landing() const + { + return false; + } + + // mode requires terrain to be present to be functional + virtual bool requires_terrain_failsafe() const + { + return false; + } + + // functions for reporting to GCS + virtual bool get_wp(Location &loc) + { + return false; + }; + virtual int32_t wp_bearing() const + { + return 0; + } + virtual uint32_t wp_distance() const + { + return 0; + } + virtual float crosstrack_error() const + { + return 0.0f; + } + + void update_navigation(); + + int32_t get_alt_above_ground_cm(void); + + // pilot input processing + void get_pilot_desired_accelerations(float &right_out, float &front_out) const; + float get_pilot_desired_yaw_rate(int16_t stick_angle); + float get_pilot_desired_throttle() const; + + // returns climb target_rate reduced to avoid obstacles and + // altitude fence + float get_avoidance_adjusted_climbrate(float target_rate); + + // const Vector3f& get_desired_velocity() { + // // note that position control isn't used in every mode, so + // // this may return bogus data: + // return pos_control->get_desired_velocity(); + // } + +protected: + + // navigation support functions + virtual void run_autopilot() {} + + // helper functions + bool is_disarmed_or_landed() const; + void zero_throttle_and_relax_ac(bool spool_up = false); + void zero_throttle_and_hold_attitude(); + void make_safe_spool_down(); + + // functions to control landing + // in modes that support landing + void land_run_horizontal_control(); + void land_run_vertical_control(bool pause_descent = false); + + // return expected input throttle setting to hover: + virtual float throttle_hover() const; + + // Alt_Hold based flight mode states used in Alt_Hold, Loiter, and Sport + // enum AltHoldModeState { + // AltHold_MotorStopped, + // AltHold_Takeoff, + // AltHold_Landed_Ground_Idle, + // AltHold_Landed_Pre_Takeoff, + // AltHold_Flying + // }; + // AltHoldModeState get_alt_hold_state(float target_climb_rate_cms); + + // convenience references to avoid code churn in conversion: + Parameters &g; + ParametersG2 &g2; + // AC_WPNav *&wp_nav; + // AC_Loiter *&loiter_nav; + AP_InertialNav &inertial_nav; + AP_AHRS &ahrs; + // AC_AttitudeControl_t *&attitude_control; + Fins *&motors; + // Fins *&motors; + RC_Channel *&channel_right; + RC_Channel *&channel_front; + RC_Channel *&channel_down; + RC_Channel *&channel_yaw; + float &G_Dt; + + // note that we support two entirely different automatic takeoffs: + + // "user-takeoff", which is available in modes such as ALT_HOLD + // (see has_user_takeoff method). "user-takeoff" is a simple + // reach-altitude-based-on-pilot-input-or-parameter routine. + + // "auto-takeoff" is used by both Guided and Auto, and is + // basically waypoint navigation with pilot yaw permitted. + + // user-takeoff support; takeoff state is shared across all mode instances + // class _TakeOff { + // public: + // void start(float alt_cm); + // void stop(); + // void get_climb_rates(float& pilot_climb_rate, + // float& takeoff_climb_rate); + // bool triggered(float target_climb_rate) const; + + // bool running() const { return _running; } + // private: + // bool _running; + // float max_speed; + // float alt_delta; + // uint32_t start_ms; + // }; + + // static _TakeOff takeoff; + + // virtual bool do_user_takeoff_start(float takeoff_alt_cm); + + // method shared by both Guided and Auto for takeoff. This is + // waypoint navigation but the user can control the yaw. + // void auto_takeoff_run(); + // void auto_takeoff_set_start_alt(void); + + // altitude above-ekf-origin below which auto takeoff does not control horizontal position + // static bool auto_takeoff_no_nav_active; + // static float auto_takeoff_no_nav_alt_cm; + +public: + // Navigation Yaw control + class AutoYaw + { + + public: + + // yaw(): main product of AutoYaw; the heading: + float yaw(); + + // mode(): current method of determining desired yaw: + autopilot_yaw_mode mode() const + { + return (autopilot_yaw_mode)_mode; + } + void set_mode_to_default(bool rtl); + void set_mode(autopilot_yaw_mode new_mode); + autopilot_yaw_mode default_mode(bool rtl) const; + + // rate_cds(): desired yaw rate in centidegrees/second: + float rate_cds() const; + void set_rate(float new_rate_cds); + + // set_roi(...): set a "look at" location: + void set_roi(const Location &roi_location); + + void set_fixed_yaw(float angle_deg, + float turn_rate_dps, + int8_t direction, + bool relative_angle); + + private: + + float look_ahead_yaw(); + float roi_yaw(); + + // auto flight mode's yaw mode + uint8_t _mode = AUTO_YAW_LOOK_AT_NEXT_WP; + + // Yaw will point at this location if mode is set to AUTO_YAW_ROI + Vector3f roi; + + // bearing from current location to the ROI + float _roi_yaw; + + // yaw used for YAW_FIXED yaw_mode + int32_t _fixed_yaw; + + // Deg/s we should turn + int16_t _fixed_yaw_slewrate; + + // heading when in yaw_look_ahead_yaw + float _look_ahead_yaw; + + // turn rate (in cds) when auto_yaw_mode is set to AUTO_YAW_RATE + float _rate_cds; + + // used to reduce update rate to 100hz: + uint8_t roi_yaw_counter; + + }; + static AutoYaw auto_yaw; + + // pass-through functions to reduce code churn on conversion; + // these are candidates for moving into the Mode base + // class. + float get_pilot_desired_climb_rate(float throttle_control); + float get_non_takeoff_throttle(void); + bool set_mode(Mode::Number mode, ModeReason reason); + void set_land_complete(bool b); + GCS_Blimp &gcs(); + void set_throttle_takeoff(void); + uint16_t get_pilot_speed_dn(void); + + // end pass-through functions +}; + +class ModeManual : public Mode +{ + +public: + // inherit constructor + using Mode::Mode; + + virtual void run() override; + + bool requires_GPS() const override + { + return false; + } + bool has_manual_throttle() const override + { + return true; + } + bool allows_arming(bool from_gcs) const override + { + return true; + }; + bool is_autopilot() const override + { + return false; + } + +protected: + + const char *name() const override + { + return "MANUAL"; + } + const char *name4() const override + { + return "MANU"; + } + +private: + +}; + +class ModeLand : public Mode +{ + +public: + // inherit constructor + using Mode::Mode; + + virtual void run() override; + + bool requires_GPS() const override + { + return false; + } + bool has_manual_throttle() const override + { + return true; + } + bool allows_arming(bool from_gcs) const override + { + return false; + }; + bool is_autopilot() const override + { + return false; + } + +protected: + + const char *name() const override + { + return "LAND"; + } + const char *name4() const override + { + return "LAND"; + } + +private: + +}; diff --git a/Blimp/mode_land.cpp b/Blimp/mode_land.cpp new file mode 100644 index 0000000000..4b6f3b5cd3 --- /dev/null +++ b/Blimp/mode_land.cpp @@ -0,0 +1,24 @@ +#include "Blimp.h" + +/* + * Init and run calls for stabilize flight mode + */ + +// manual_run - runs the main manual controller +// should be called at 100hz or more +void ModeLand::run() +{ + //stop moving + +} + +// set_mode_land_with_pause - sets mode to LAND and triggers 4 second delay before descent starts +// this is always called from a failsafe so we trigger notification to pilot +void Blimp::set_mode_land_with_pause(ModeReason reason) +{ + set_mode(Mode::Number::LAND, reason); + // land_pause = true; + + // alert pilot to mode change + AP_Notify::events.failsafe_mode_change = 1; +} \ No newline at end of file diff --git a/Blimp/mode_manual.cpp b/Blimp/mode_manual.cpp new file mode 100644 index 0000000000..a2a88360a9 --- /dev/null +++ b/Blimp/mode_manual.cpp @@ -0,0 +1,29 @@ +#include "Blimp.h" +/* + * Init and run calls for stabilize flight mode + */ + +// manual_run - runs the main manual controller +// should be called at 100hz or more +void ModeManual::run() +{ + // convert pilot input to lean angles + // float target_right, target_front; + // get_pilot_desired_accelerations(target_right, target_front); + + motors->right_out = channel_right->get_control_in(); + motors->front_out = channel_front->get_control_in(); + motors->yaw_out = channel_yaw->get_control_in(); + motors->down_out = channel_down->get_control_in(); + + if (!motors->armed()) { + // Motors should be Stopped + motors->set_desired_spool_state(Fins::DesiredSpoolState::SHUT_DOWN); + } else { + motors->set_desired_spool_state(Fins::DesiredSpoolState::THROTTLE_UNLIMITED); + } + + + + // motors->output(); //MIR need to add sending direction & throttle commands. +} diff --git a/Blimp/motors.cpp b/Blimp/motors.cpp new file mode 100644 index 0000000000..c203477488 --- /dev/null +++ b/Blimp/motors.cpp @@ -0,0 +1,92 @@ +#include "Blimp.h" + +#define ARM_DELAY 20 // called at 10hz so 2 seconds +#define DISARM_DELAY 20 // called at 10hz so 2 seconds +#define AUTO_TRIM_DELAY 100 // called at 10hz so 10 seconds +#define LOST_VEHICLE_DELAY 10 // called at 10hz so 1 second + +// arm_motors_check - checks for pilot input to arm or disarm the blimp +// called at 10hz +void Blimp::arm_motors_check() +{ + static int16_t arming_counter; + + // check if arming/disarm using rudder is allowed + AP_Arming::RudderArming arming_rudder = arming.get_rudder_arming_type(); + if (arming_rudder == AP_Arming::RudderArming::IS_DISABLED) { + return; + } + + // ensure throttle is down + if (channel_down->get_control_in() > 0) { //MIR what dow we do with this? + arming_counter = 0; + return; + } + + int16_t yaw_in = channel_yaw->get_control_in(); + + // full right + if (yaw_in > 900) { + + // increase the arming counter to a maximum of 1 beyond the auto trim counter + if (arming_counter <= AUTO_TRIM_DELAY) { + arming_counter++; + } + + // arm the motors and configure for flight + if (arming_counter == ARM_DELAY && !motors->armed()) { + // reset arming counter if arming fail + if (!arming.arm(AP_Arming::Method::RUDDER)) { + arming_counter = 0; + } + } + + // arm the motors and configure for flight + if (arming_counter == AUTO_TRIM_DELAY && motors->armed() && control_mode == Mode::Number::MANUAL) { + gcs().send_text(MAV_SEVERITY_INFO, "AutoTrim start"); + auto_trim_counter = 250; + auto_trim_started = false; + } + + // full left and rudder disarming is enabled + } else if ((yaw_in < -900) && (arming_rudder == AP_Arming::RudderArming::ARMDISARM)) { + if (!flightmode->has_manual_throttle() && !ap.land_complete) { + arming_counter = 0; + return; + } + + // increase the counter to a maximum of 1 beyond the disarm delay + if (arming_counter <= DISARM_DELAY) { + arming_counter++; + } + + // disarm the motors + if (arming_counter == DISARM_DELAY && motors->armed()) { + arming.disarm(AP_Arming::Method::RUDDER); + } + + // Yaw is centered so reset arming counter + } else { + arming_counter = 0; + } +} + + +// motors_output - send output to motors library which will adjust and send to ESCs and servos +void Blimp::motors_output() +{ + // output any servo channels + SRV_Channels::calc_pwm(); + + // cork now, so that all channel outputs happen at once + SRV_Channels::cork(); + + // update output on any aux channels, for manual passthru + SRV_Channels::output_ch_all(); + + // send output signals to motors + motors->output(); + + // push all channels + SRV_Channels::push(); +} \ No newline at end of file diff --git a/Blimp/radio.cpp b/Blimp/radio.cpp new file mode 100644 index 0000000000..6b0821dac2 --- /dev/null +++ b/Blimp/radio.cpp @@ -0,0 +1,192 @@ +#include "Blimp.h" + + +// Function that will read the radio data, limit servos and trigger a failsafe +// ---------------------------------------------------------------------------- + +void Blimp::default_dead_zones() +{ + channel_right->set_default_dead_zone(20); + channel_front->set_default_dead_zone(20); + channel_down->set_default_dead_zone(30); + channel_yaw->set_default_dead_zone(20); + rc().channel(CH_6)->set_default_dead_zone(0); +} + +void Blimp::init_rc_in() +{ + channel_right = rc().channel(rcmap.roll()-1); + channel_front = rc().channel(rcmap.pitch()-1); + channel_down = rc().channel(rcmap.throttle()-1); + channel_yaw = rc().channel(rcmap.yaw()-1); + + // set rc channel ranges + channel_right->set_angle(RC_SCALE); + channel_front->set_angle(RC_SCALE); + channel_yaw->set_angle(RC_SCALE); + channel_down->set_angle(RC_SCALE); + + // set default dead zones + default_dead_zones(); + + // initialise throttle_zero flag + ap.throttle_zero = true; +} + +// init_rc_out -- initialise motors +void Blimp::init_rc_out() +{ + // motors->init((AP_Motors::motor_frame_class)g2.frame_class.get(), (AP_Motors::motor_frame_type)g.frame_type.get()); + // MIR will need motors->init later when we switch to other fin config. + + // enable aux servos to cope with multiple output channels per motor + SRV_Channels::enable_aux_servos(); + + // update rate must be set after motors->init() to allow for motor mapping + // motors->set_update_rate(g.rc_speed); + + // motors->set_throttle_range(channel_down->get_radio_min(), channel_down->get_radio_max()); + + // refresh auxiliary channel to function map + SRV_Channels::update_aux_servo_function(); + + /* + setup a default safety ignore mask, so that servo gimbals can be active while safety is on + */ + // uint16_t safety_ignore_mask = (~blimp.motors->get_motor_mask()) & 0x3FFF; + // BoardConfig.set_default_safety_ignore_mask(safety_ignore_mask); +} + + +// enable_motor_output() - enable and output lowest possible value to motors +void Blimp::enable_motor_output() +{ + // enable motors + motors->output_min(); +} + +void Blimp::read_radio() +{ + const uint32_t tnow_ms = millis(); + + if (rc().read_input()) { + ap.new_radio_frame = true; + + set_throttle_and_failsafe(channel_down->get_radio_in()); + set_throttle_zero_flag(channel_down->get_control_in()); + + // RC receiver must be attached if we've just got input + ap.rc_receiver_present = true; + + // pass pilot input through to motors (used to allow wiggling servos while disarmed on heli, single, coax blimps) + // radio_passthrough_to_motors(); + + const float dt = (tnow_ms - last_radio_update_ms)*1.0e-3f; + rc_throttle_control_in_filter.apply(channel_down->get_control_in(), dt); + last_radio_update_ms = tnow_ms; + return; + } + + // No radio input this time + if (failsafe.radio) { + // already in failsafe! + return; + } + + const uint32_t elapsed = tnow_ms - last_radio_update_ms; + // turn on throttle failsafe if no update from the RC Radio for 500ms or 2000ms if we are using RC_OVERRIDE + const uint32_t timeout = RC_Channels::has_active_overrides() ? FS_RADIO_RC_OVERRIDE_TIMEOUT_MS : FS_RADIO_TIMEOUT_MS; + if (elapsed < timeout) { + // not timed out yet + return; + } + if (!g.failsafe_throttle) { + // throttle failsafe not enabled + return; + } + if (!ap.rc_receiver_present && !motors->armed()) { + // we only failsafe if we are armed OR we have ever seen an RC receiver + return; + } + + // Nobody ever talks to us. Log an error and enter failsafe. + AP::logger().Write_Error(LogErrorSubsystem::RADIO, LogErrorCode::RADIO_LATE_FRAME); + set_failsafe_radio(true); +} + +#define FS_COUNTER 3 // radio failsafe kicks in after 3 consecutive throttle values below failsafe_throttle_value +void Blimp::set_throttle_and_failsafe(uint16_t throttle_pwm) +{ + // if failsafe not enabled pass through throttle and exit + if (g.failsafe_throttle == FS_THR_DISABLED) { + return; + } + + //check for low throttle value + if (throttle_pwm < (uint16_t)g.failsafe_throttle_value) { + + // if we are already in failsafe or motors not armed pass through throttle and exit + if (failsafe.radio || !(ap.rc_receiver_present || motors->armed())) { + return; + } + + // check for 3 low throttle values + // Note: we do not pass through the low throttle until 3 low throttle values are received + failsafe.radio_counter++; + if ( failsafe.radio_counter >= FS_COUNTER ) { + failsafe.radio_counter = FS_COUNTER; // check to ensure we don't overflow the counter + set_failsafe_radio(true); + } + } else { + // we have a good throttle so reduce failsafe counter + failsafe.radio_counter--; + if ( failsafe.radio_counter <= 0 ) { + failsafe.radio_counter = 0; // check to ensure we don't underflow the counter + + // disengage failsafe after three (nearly) consecutive valid throttle values + if (failsafe.radio) { + set_failsafe_radio(false); + } + } + // pass through throttle + } +} + +#define THROTTLE_ZERO_DEBOUNCE_TIME_MS 400 +// set_throttle_zero_flag - set throttle_zero flag from debounced throttle control +// throttle_zero is used to determine if the pilot intends to shut down the motors +// Basically, this signals when we are not flying. We are either on the ground +// or the pilot has shut down the blimp in the air and it is free-falling +void Blimp::set_throttle_zero_flag(int16_t throttle_control) +{ + static uint32_t last_nonzero_throttle_ms = 0; + uint32_t tnow_ms = millis(); + + // if not using throttle interlock and non-zero throttle and not E-stopped, + // or using motor interlock and it's enabled, then motors are running, + // and we are flying. Immediately set as non-zero + if (throttle_control > 0) { + last_nonzero_throttle_ms = tnow_ms; + ap.throttle_zero = false; + } else if (tnow_ms - last_nonzero_throttle_ms > THROTTLE_ZERO_DEBOUNCE_TIME_MS) { + ap.throttle_zero = true; + } + //MIR What does this mean?? +} + +// pass pilot's inputs to motors library (used to allow wiggling servos while disarmed on heli, single, coax blimps) +// void Blimp::radio_passthrough_to_motors() +// { +// motors->set_radio_passthrough(channel_right->norm_input(), +// channel_front->norm_input(), +// channel_down->get_control_in_zero_dz()*0.001f, +// channel_yaw->norm_input()); +// } + +/* + return the throttle input for mid-stick as a control-in value + */ +int16_t Blimp::get_throttle_mid(void) +{ + return channel_down->get_control_mid(); +} diff --git a/Blimp/sensors.cpp b/Blimp/sensors.cpp new file mode 100644 index 0000000000..84586e1e1c --- /dev/null +++ b/Blimp/sensors.cpp @@ -0,0 +1,62 @@ +#include "Blimp.h" + +// return barometric altitude in centimeters +void Blimp::read_barometer(void) +{ + barometer.update(); + + baro_alt = barometer.get_altitude() * 100.0f; + + motors->set_air_density_ratio(barometer.get_air_density_ratio()); +} + + +void Blimp::compass_cal_update() +{ + compass.cal_update(); + + if (hal.util->get_soft_armed()) { + return; + } + + static uint32_t compass_cal_stick_gesture_begin = 0; + + if (compass.is_calibrating()) { + if (channel_yaw->get_control_in() < -4000 && channel_down->get_control_in() > 900) { + compass.cancel_calibration_all(); + } + } else { + bool stick_gesture_detected = compass_cal_stick_gesture_begin != 0 && !motors->armed() && channel_yaw->get_control_in() > 4000 && channel_down->get_control_in() > 900; + uint32_t tnow = millis(); + + if (!stick_gesture_detected) { + compass_cal_stick_gesture_begin = tnow; + } else if (tnow-compass_cal_stick_gesture_begin > 1000*COMPASS_CAL_STICK_GESTURE_TIME) { +#ifdef CAL_ALWAYS_REBOOT + compass.start_calibration_all(true,true,COMPASS_CAL_STICK_DELAY,true); +#else + compass.start_calibration_all(true,true,COMPASS_CAL_STICK_DELAY,false); +#endif + } + } +} + +void Blimp::accel_cal_update() +{ + if (hal.util->get_soft_armed()) { + return; + } + ins.acal_update(); + // check if new trim values, and set them + float trim_roll, trim_pitch; + if (ins.get_new_trim(trim_roll, trim_pitch)) { + ahrs.set_trim(Vector3f(trim_roll, trim_pitch, 0)); + } + +#ifdef CAL_ALWAYS_REBOOT + if (ins.accel_cal_requires_reboot()) { + hal.scheduler->delay(1000); + hal.scheduler->reboot(false); + } +#endif +} diff --git a/Blimp/system.cpp b/Blimp/system.cpp new file mode 100644 index 0000000000..980f12ec9d --- /dev/null +++ b/Blimp/system.cpp @@ -0,0 +1,358 @@ +#include "Blimp.h" + +/***************************************************************************** +* The init_ardupilot function processes everything we need for an in - air restart +* We will determine later if we are actually on the ground and process a +* ground start in that case. +* +*****************************************************************************/ + +static void failsafe_check_static() +{ + blimp.failsafe_check(); +} + +void Blimp::init_ardupilot() +{ + +#if STATS_ENABLED == ENABLED + // initialise stats module + g2.stats.init(); +#endif + + BoardConfig.init(); + + + // initialise notify system + notify.init(); + notify_flight_mode(); + + // initialise battery monitor + battery.init(); + + // Init RSSI + rssi.init(); + + barometer.init(); + + // setup telem slots with serial ports + gcs().setup_uarts(); + +#if LOGGING_ENABLED == ENABLED + log_init(); +#endif + + // update motor interlock state + // update_using_interlock(); + + + init_rc_in(); // sets up rc channels from radio + + // allocate the motors class + allocate_motors(); + + // initialise rc channels including setting mode + rc().init(); + + // sets up motors and output to escs + init_rc_out(); + + + // motors initialised so parameters can be sent + ap.initialised_params = true; + + relay.init(); + + /* + * setup the 'main loop is dead' check. Note that this relies on + * the RC library being initialised. + */ + hal.scheduler->register_timer_failsafe(failsafe_check_static, 1000); + + // Do GPS init + gps.set_log_gps_bit(MASK_LOG_GPS); + gps.init(serial_manager); + + AP::compass().set_log_bit(MASK_LOG_COMPASS); + AP::compass().init(); + + // attitude_control->parameter_sanity_check(); + // pos_control->set_dt(scheduler.get_loop_period_s()); + +#if HIL_MODE != HIL_MODE_DISABLED + while (barometer.get_last_update() == 0) { + // the barometer begins updating when we get the first + // HIL_STATE message + gcs().send_text(MAV_SEVERITY_WARNING, "Waiting for first HIL_STATE message"); + delay(1000); + } + + // set INS to HIL mode + ins.set_hil_mode(); +#endif + + // read Baro pressure at ground + //----------------------------- + barometer.set_log_baro_bit(MASK_LOG_IMU); + barometer.calibrate(); + + // #if MODE_AUTO_ENABLED == ENABLED + // // initialise mission library + // mode_auto.mission.init(); + // #endif + + // initialise AP_Logger library + logger.setVehicle_Startup_Writer(FUNCTOR_BIND(&blimp, &Blimp::Log_Write_Vehicle_Startup_Messages, void)); + + startup_INS_ground(); + +#ifdef ENABLE_SCRIPTING + g2.scripting.init(); +#endif // ENABLE_SCRIPTING + + // set landed flags + // set_land_complete(true); + // set_land_complete_maybe(true); + + // we don't want writes to the serial port to cause us to pause + // mid-flight, so set the serial ports non-blocking once we are + // ready to fly + serial_manager.set_blocking_writes_all(false); + + // enable CPU failsafe + // failsafe_enable(); + + ins.set_log_raw_bit(MASK_LOG_IMU_RAW); + + // setup fin output + motors->setup_fins(); + + // enable output to motors + if (arming.rc_calibration_checks(true)) { + enable_motor_output(); + } + + // attempt to switch to MANUAL, if this fails then switch to Land + if (!set_mode((enum Mode::Number)g.initial_mode.get(), ModeReason::INITIALISED)) { + // set mode to MANUAL will trigger mode change notification to pilot + set_mode(Mode::Number::MANUAL, ModeReason::UNAVAILABLE); + } else { + // alert pilot to mode change + AP_Notify::events.failsafe_mode_change = 1; + } + + // flag that initialisation has completed + ap.initialised = true; +} + + +//****************************************************************************** +//This function does all the calibrations, etc. that we need during a ground start +//****************************************************************************** +void Blimp::startup_INS_ground() +{ + // initialise ahrs (may push imu calibration into the mpu6000 if using that device). + ahrs.init(); + ahrs.set_vehicle_class(AHRS_VEHICLE_COPTER); + + // Warm up and calibrate gyro offsets + ins.init(scheduler.get_loop_rate_hz()); + + // reset ahrs including gyro bias + ahrs.reset(); +} + +// position_ok - returns true if the horizontal absolute position is ok and home position is set +bool Blimp::position_ok() const +{ + // return false if ekf failsafe has triggered + if (failsafe.ekf) { + return false; + } + + // check ekf position estimate + return (ekf_has_absolute_position() || ekf_has_relative_position()); +} + +// ekf_has_absolute_position - returns true if the EKF can provide an absolute WGS-84 position estimate +bool Blimp::ekf_has_absolute_position() const +{ + if (!ahrs.have_inertial_nav()) { + // do not allow navigation with dcm position + return false; + } + + // with EKF use filter status and ekf check + nav_filter_status filt_status = inertial_nav.get_filter_status(); + + // if disarmed we accept a predicted horizontal position + if (!motors->armed()) { + return ((filt_status.flags.horiz_pos_abs || filt_status.flags.pred_horiz_pos_abs)); + } else { + // once armed we require a good absolute position and EKF must not be in const_pos_mode + return (filt_status.flags.horiz_pos_abs && !filt_status.flags.const_pos_mode); + } +} + +// ekf_has_relative_position - returns true if the EKF can provide a position estimate relative to it's starting position +bool Blimp::ekf_has_relative_position() const +{ + // return immediately if EKF not used + if (!ahrs.have_inertial_nav()) { + return false; + } + + // return immediately if neither optflow nor visual odometry is enabled + bool enabled = false; + // #if OPTFLOW == ENABLED + // if (optflow.enabled()) { + // enabled = true; + // } + // #endif + // #if HAL_VISUALODOM_ENABLED + // if (visual_odom.enabled()) { + // enabled = true; + // } + // #endif + if (!enabled) { + return false; + } + + // get filter status from EKF + nav_filter_status filt_status = inertial_nav.get_filter_status(); + + // if disarmed we accept a predicted horizontal relative position + if (!motors->armed()) { + return (filt_status.flags.pred_horiz_pos_rel); + } else { + return (filt_status.flags.horiz_pos_rel && !filt_status.flags.const_pos_mode); + } +} + +// returns true if the ekf has a good altitude estimate (required for modes which do AltHold) +bool Blimp::ekf_alt_ok() const +{ + if (!ahrs.have_inertial_nav()) { + // do not allow alt control with only dcm + return false; + } + + // with EKF use filter status and ekf check + nav_filter_status filt_status = inertial_nav.get_filter_status(); + + // require both vertical velocity and position + return (filt_status.flags.vert_vel && filt_status.flags.vert_pos); +} + +// update_auto_armed - update status of auto_armed flag +void Blimp::update_auto_armed() +{ + // disarm checks + if (ap.auto_armed) { + // if motors are disarmed, auto_armed should also be false + if (!motors->armed()) { + set_auto_armed(false); + return; + } + // if in stabilize or acro flight mode and throttle is zero, auto-armed should become false + if (flightmode->has_manual_throttle() && ap.throttle_zero && !failsafe.radio) { + set_auto_armed(false); + } + // if heliblimps are on the ground, and the motor is switched off, auto-armed should be false + // so that rotor runup is checked again before attempting to take-off + if (ap.land_complete && motors->get_spool_state() != Fins::SpoolState::THROTTLE_UNLIMITED && ap.using_interlock) { + set_auto_armed(false); + } + } else { + // arm checks + + // // for tradheli if motors are armed and throttle is above zero and the motor is started, auto_armed should be true + // if(motors->armed() && ap.using_interlock) { + // if(!ap.throttle_zero && motors->get_spool_state() == Fins::SpoolState::THROTTLE_UNLIMITED) { + // set_auto_armed(true); + // } + // // if motors are armed and throttle is above zero auto_armed should be true + // // if motors are armed and we are in throw mode, then auto_armed should be true + // } else if (motors->armed() && !ap.using_interlock) { + // if(!ap.throttle_zero) { + // set_auto_armed(true); + // } + // } + } +} + +/* + should we log a message type now? + */ +bool Blimp::should_log(uint32_t mask) +{ +#if LOGGING_ENABLED == ENABLED + ap.logging_started = logger.logging_started(); + return logger.should_log(mask); +#else + return false; +#endif +} + +// return MAV_TYPE corresponding to frame class +MAV_TYPE Blimp::get_frame_mav_type() +{ + return MAV_TYPE_QUADROTOR; //MIR changed to this for now - will need to deal with mavlink changes later +} + +// return string corresponding to frame_class +const char* Blimp::get_frame_string() +{ + return "AIRFISH"; +} + +/* + allocate the motors class + */ +void Blimp::allocate_motors(void) +{ + switch ((Fins::motor_frame_class)g2.frame_class.get()) { + case Fins::MOTOR_FRAME_AIRFISH: + default: + motors = new Fins(blimp.scheduler.get_loop_rate_hz()); + break; + } + if (motors == nullptr) { + AP_HAL::panic("Unable to allocate FRAME_CLASS=%u", (unsigned)g2.frame_class.get()); + } + AP_Param::load_object_from_eeprom(motors, Fins::var_info); + + // const struct AP_Param::GroupInfo *ac_var_info; + + // attitude_control = new AC_AttitudeControl_Multi(*ahrs_view, aparm, *motors, scheduler.get_loop_period_s()); + // ac_var_info = AC_AttitudeControl_Multi::var_info; + // if (attitude_control == nullptr) { + // AP_HAL::panic("Unable to allocate AttitudeControl"); + // } + // AP_Param::load_object_from_eeprom(attitude_control, ac_var_info); + + // pos_control = new AC_PosControl(*ahrs_view, inertial_nav, *motors, *attitude_control); + // if (pos_control == nullptr) { + // AP_HAL::panic("Unable to allocate PosControl"); + // } + // AP_Param::load_object_from_eeprom(pos_control, pos_control->var_info); + + // wp_nav = new AC_WPNav(inertial_nav, *ahrs_view, *pos_control, *attitude_control); + + // if (wp_nav == nullptr) { + // AP_HAL::panic("Unable to allocate WPNav"); + // } + // AP_Param::load_object_from_eeprom(wp_nav, wp_nav->var_info); + + // loiter_nav = new AC_Loiter(inertial_nav, *ahrs_view, *pos_control, *attitude_control); + // if (loiter_nav == nullptr) { + // AP_HAL::panic("Unable to allocate LoiterNav"); + // } + // AP_Param::load_object_from_eeprom(loiter_nav, loiter_nav->var_info); + + // reload lines from the defaults file that may now be accessible + AP_Param::reload_defaults_file(true); + + // param count could have changed + AP_Param::invalidate_count(); +} diff --git a/Blimp/version.h b/Blimp/version.h new file mode 100644 index 0000000000..aa97bacfe2 --- /dev/null +++ b/Blimp/version.h @@ -0,0 +1,19 @@ +#pragma once + +#ifndef FORCE_VERSION_H_INCLUDE +#error version.h should never be included directly. You probably want to include AP_Common/AP_FWVersion.h +#endif + +#include "ap_version.h" + +#define THISFIRMWARE "Blimp V4.1.0-dev" + +// the following line is parsed by the autotest scripts +#define FIRMWARE_VERSION 4,1,0,FIRMWARE_VERSION_TYPE_DEV + +#define FW_MAJOR 4 +#define FW_MINOR 1 +#define FW_PATCH 0 +#define FW_TYPE FIRMWARE_VERSION_TYPE_DEV + +#include \ No newline at end of file diff --git a/Blimp/wscript b/Blimp/wscript new file mode 100644 index 0000000000..ea08530213 --- /dev/null +++ b/Blimp/wscript @@ -0,0 +1,31 @@ +#!/usr/bin/env python +# encoding: utf-8 + +def build(bld): + vehicle = bld.path.name + bld.ap_stlib( + name=vehicle + '_libs', + ap_vehicle=vehicle, + ap_libraries=bld.ap_common_vehicle_libraries() + [ + 'AC_AttitudeControl', + 'AC_InputManager', + 'AC_WPNav', + 'AP_InertialNav', + 'AP_RCMapper', + 'AP_Avoidance', + 'AP_Arming', + 'AP_LTM_Telem', + 'AP_Devo_Telem', + 'AP_OSD', + 'AP_KDECAN', + 'AP_Beacon', + 'AP_AdvancedFailsafe', #for some reason compiling GCS_Common.cpp (in libraries) fails if this isn't included... + ], + ) + + bld.ap_program( + program_name='blimp', + program_groups=['bin', 'blimp'], + use=vehicle + '_libs', + defines=['FRAME_CONFIG=MULTICOPTER_FRAME'], + )