From ded488fd9f72d099a24126866ec751d203eaf2d8 Mon Sep 17 00:00:00 2001 From: Michelle Rossouw <64898873+MichelleRos@users.noreply.github.com> Date: Tue, 30 Mar 2021 01:43:54 +1100 Subject: [PATCH] Blimp: Remove most commented out code and other cleanups --- Blimp/AP_Arming.cpp | 17 ---- Blimp/AP_Arming.h | 1 - Blimp/Blimp.cpp | 7 -- Blimp/Blimp.h | 5 -- Blimp/Fins.cpp | 11 ++- Blimp/Fins.h | 55 ++---------- Blimp/GCS_Blimp.cpp | 23 ----- Blimp/GCS_Mavlink.cpp | 18 +--- Blimp/GCS_Mavlink.h | 4 +- Blimp/Log.cpp | 86 +------------------ Blimp/Parameters.cpp | 56 ------------- Blimp/Parameters.h | 11 --- Blimp/commands.cpp | 10 --- Blimp/defines.h | 16 ---- Blimp/ekf_check.cpp | 7 -- Blimp/events.cpp | 191 ------------------------------------------ Blimp/mode.cpp | 174 +------------------------------------- Blimp/mode.h | 53 ------------ Blimp/mode_land.cpp | 2 +- Blimp/mode_manual.cpp | 6 -- Blimp/radio.cpp | 28 +------ Blimp/sensors.cpp | 2 - Blimp/system.cpp | 23 +---- Blimp/wscript | 2 +- 24 files changed, 25 insertions(+), 783 deletions(-) diff --git a/Blimp/AP_Arming.cpp b/Blimp/AP_Arming.cpp index f8505fbfdb..7154c8a9bc 100644 --- a/Blimp/AP_Arming.cpp +++ b/Blimp/AP_Arming.cpp @@ -170,23 +170,6 @@ bool AP_Arming_Blimp::parameter_checks(bool display_failure) 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; diff --git a/Blimp/AP_Arming.h b/Blimp/AP_Arming.h index a6f5066a97..6dbe21c1bb 100644 --- a/Blimp/AP_Arming.h +++ b/Blimp/AP_Arming.h @@ -30,7 +30,6 @@ 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 diff --git a/Blimp/Blimp.cpp b/Blimp/Blimp.cpp index a9963e91ee..8ac770870a 100644 --- a/Blimp/Blimp.cpp +++ b/Blimp/Blimp.cpp @@ -201,9 +201,6 @@ void Blimp::rc_loop() // --------------------------- 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(); } @@ -217,7 +214,6 @@ void Blimp::update_batt_compass(void) 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(); } @@ -287,9 +283,6 @@ 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 diff --git a/Blimp/Blimp.h b/Blimp/Blimp.h index 54a57a9ae6..d3f7cd1b8c 100644 --- a/Blimp/Blimp.h +++ b/Blimp/Blimp.h @@ -482,7 +482,6 @@ private: 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 @@ -507,7 +506,6 @@ private: // 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; @@ -518,9 +516,6 @@ private: 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; diff --git a/Blimp/Fins.cpp b/Blimp/Fins.cpp index c665a40ab3..563a5607e4 100644 --- a/Blimp/Fins.cpp +++ b/Blimp/Fins.cpp @@ -1,8 +1,6 @@ #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 - +// This is the scale used for RC inputs so that they can be scaled to the float point values used in the sin wave code. #define FIN_SCALE_MAX 1000 /* @@ -119,17 +117,18 @@ void Fins::output() } if (turbo_mode) { - //double speed fins if offset at max... MIR + //double speed fins if offset at max... 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 + _pos[i]= _amp[i]*cosf(freq_hz * _omm[i] * _time * 2 * M_PI) + _off[i]; SRV_Channels::set_output_scaled(SRV_Channels::get_motor_function(i), _pos[i] * FIN_SCALE_MAX); } + //For debugging purposes. Displays in the debug terminal so it doesn't flood the GCS. ::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]); } @@ -143,7 +142,7 @@ void Fins::output_min() Fins::output(); } -// MIR - Probably want to completely get rid of the desired spool state thing. +// TODO - 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)) { diff --git a/Blimp/Fins.h b/Blimp/Fins.h index 2994c8b9e5..e546f5b4b4 100644 --- a/Blimp/Fins.h +++ b/Blimp/Fins.h @@ -1,23 +1,16 @@ //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 -// #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 NUM_FINS 4 //Current maximum number of fins that can be added. #define RC_SCALE 1000 class Fins { public: friend class Blimp; - // Fins(void); enum motor_frame_class { MOTOR_FRAME_UNDEFINED = 0, @@ -33,24 +26,14 @@ public: // 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) + SHUT_DOWN = 0, // all fins should move to stop + THROTTLE_UNLIMITED = 2, // all fins can move as needed }; - // 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 @@ -75,23 +58,10 @@ 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 @@ -112,7 +82,8 @@ protected: float _yaw_off_factor[NUM_FINS]; int8_t _num_added; - // private: + +//MIR This should probably become private in future. 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 @@ -131,9 +102,6 @@ public: 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) { @@ -152,7 +120,7 @@ public: float get_throttle_hover() { - return 0; //MIR temp + return 0; //TODO } void set_desired_spool_state(DesiredSpoolState spool); @@ -161,15 +129,8 @@ public: float get_throttle() { - return 0.1f; //MIR temp + return 0.1f; //TODO } 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 index c273c3f0ba..fce068a77a 100644 --- a/Blimp/GCS_Blimp.cpp +++ b/Blimp/GCS_Blimp.cpp @@ -39,29 +39,6 @@ void GCS_Blimp::update_vehicle_sensor_status_flags(void) 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_Mavlink.cpp b/Blimp/GCS_Mavlink.cpp index a467c505ab..102df39083 100644 --- a/Blimp/GCS_Mavlink.cpp +++ b/Blimp/GCS_Mavlink.cpp @@ -157,21 +157,7 @@ void GCS_MAVLINK_Blimp::send_position_target_global_int() 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 @@ -212,7 +198,7 @@ void GCS_MAVLINK_Blimp::send_pid_tuning() return; } const AP_Logger::PID_Info *pid_info = nullptr; - switch (axes[i]) { //MIR Temp + switch (axes[i]) { //TODO This should probably become an acceleration controller? // case PID_TUNING_ROLL: // pid_info = &blimp.attitude_control->get_rate_roll_pid().get_pid_info(); // break; diff --git a/Blimp/GCS_Mavlink.h b/Blimp/GCS_Mavlink.h index 4ae1ba3057..4a2ee8f63d 100644 --- a/Blimp/GCS_Mavlink.h +++ b/Blimp/GCS_Mavlink.h @@ -24,7 +24,6 @@ protected: 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; @@ -33,11 +32,10 @@ protected: 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?? + void send_nav_controller_output() const override; //TODO Apparently can't remove this or the build fails. uint64_t capabilities() const override; virtual MAV_VTOL_STATE vtol_state() const override diff --git a/Blimp/Log.cpp b/Blimp/Log.cpp index d7ca4d2e41..392e71fc68 100644 --- a/Blimp/Log.cpp +++ b/Blimp/Log.cpp @@ -25,53 +25,13 @@ struct PACKED log_Control_Tuning { // 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 @@ -97,15 +57,7 @@ struct PACKED log_MotBatt { // 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 { @@ -330,38 +282,6 @@ time_fade_out : time_fade_out #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 diff --git a/Blimp/Parameters.cpp b/Blimp/Parameters.cpp index 62eeacef5a..f608777a37 100644 --- a/Blimp/Parameters.cpp +++ b/Blimp/Parameters.cpp @@ -99,62 +99,6 @@ const AP_Param::Info Blimp::var_info[] = { // @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. diff --git a/Blimp/Parameters.h b/Blimp/Parameters.h index f916c3374d..47c82143ba 100644 --- a/Blimp/Parameters.h +++ b/Blimp/Parameters.h @@ -469,22 +469,11 @@ public: // 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; diff --git a/Blimp/commands.cpp b/Blimp/commands.cpp index 537cf2e1e1..65a3ba886a 100644 --- a/Blimp/commands.cpp +++ b/Blimp/commands.cpp @@ -74,16 +74,6 @@ bool Blimp::set_home(const Location& loc, bool lock) 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 diff --git a/Blimp/defines.h b/Blimp/defines.h index 8936dbfe27..bd431a2471 100644 --- a/Blimp/defines.h +++ b/Blimp/defines.h @@ -107,22 +107,6 @@ enum GuidedMode { 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, diff --git a/Blimp/ekf_check.cpp b/Blimp/ekf_check.cpp index 11ec4e1178..7811aa79bb 100644 --- a/Blimp/ekf_check.cpp +++ b/Blimp/ekf_check.cpp @@ -147,13 +147,6 @@ void Blimp::failsafe_ekf_event() 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; diff --git a/Blimp/events.cpp b/Blimp/events.cpp index 44f5924132..6f77c54c91 100644 --- a/Blimp/events.cpp +++ b/Blimp/events.cpp @@ -118,198 +118,7 @@ void Blimp::failsafe_gcs_check() // 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) { diff --git a/Blimp/mode.cpp b/Blimp/mode.cpp index 9c9239d7db..c48f7d4cec 100644 --- a/Blimp/mode.cpp +++ b/Blimp/mode.cpp @@ -280,12 +280,7 @@ void Mode::zero_throttle_and_relax_ac(bool spool_up) 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; } @@ -294,113 +289,6 @@ int32_t Mode::get_alt_above_ground_cm(void) 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(); @@ -438,58 +326,6 @@ float Mode::get_pilot_desired_throttle() const 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. @@ -523,14 +359,6 @@ 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 index 1c00fa39b8..0b60f3d66d 100644 --- a/Blimp/mode.h +++ b/Blimp/mode.h @@ -149,71 +149,18 @@ protected: // 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 diff --git a/Blimp/mode_land.cpp b/Blimp/mode_land.cpp index 4b6f3b5cd3..f6bb1cbb76 100644 --- a/Blimp/mode_land.cpp +++ b/Blimp/mode_land.cpp @@ -17,7 +17,7 @@ void ModeLand::run() void Blimp::set_mode_land_with_pause(ModeReason reason) { set_mode(Mode::Number::LAND, reason); - // land_pause = true; + //TODO: Add pause // alert pilot to mode change AP_Notify::events.failsafe_mode_change = 1; diff --git a/Blimp/mode_manual.cpp b/Blimp/mode_manual.cpp index a2a88360a9..d91d60f668 100644 --- a/Blimp/mode_manual.cpp +++ b/Blimp/mode_manual.cpp @@ -7,9 +7,6 @@ // 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(); @@ -23,7 +20,4 @@ void ModeManual::run() motors->set_desired_spool_state(Fins::DesiredSpoolState::THROTTLE_UNLIMITED); } - - - // motors->output(); //MIR need to add sending direction & throttle commands. } diff --git a/Blimp/radio.cpp b/Blimp/radio.cpp index 6b0821dac2..6081dbdffd 100644 --- a/Blimp/radio.cpp +++ b/Blimp/radio.cpp @@ -36,25 +36,11 @@ void Blimp::init_rc_in() // 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); } @@ -78,9 +64,6 @@ void Blimp::read_radio() // 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; @@ -156,7 +139,7 @@ void Blimp::set_throttle_and_failsafe(uint16_t throttle_pwm) // 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 +// or the pilot has shut down the vehicle in the air and it is free-floating void Blimp::set_throttle_zero_flag(int16_t throttle_control) { static uint32_t last_nonzero_throttle_ms = 0; @@ -174,15 +157,6 @@ void Blimp::set_throttle_zero_flag(int16_t throttle_control) //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 */ diff --git a/Blimp/sensors.cpp b/Blimp/sensors.cpp index 84586e1e1c..4370519764 100644 --- a/Blimp/sensors.cpp +++ b/Blimp/sensors.cpp @@ -6,8 +6,6 @@ void Blimp::read_barometer(void) barometer.update(); baro_alt = barometer.get_altitude() * 100.0f; - - motors->set_air_density_ratio(barometer.get_air_density_ratio()); } diff --git a/Blimp/system.cpp b/Blimp/system.cpp index 980f12ec9d..79ecb3a1b6 100644 --- a/Blimp/system.cpp +++ b/Blimp/system.cpp @@ -42,10 +42,6 @@ void Blimp::init_ardupilot() log_init(); #endif - // update motor interlock state - // update_using_interlock(); - - init_rc_in(); // sets up rc channels from radio // allocate the motors class @@ -263,21 +259,6 @@ void Blimp::update_auto_armed() 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); - // } - // } } } @@ -297,13 +278,13 @@ bool Blimp::should_log(uint32_t mask) // 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 MAV_TYPE_QUADROTOR; //TODO: Mavlink changes to allow type to be correct } // return string corresponding to frame_class const char* Blimp::get_frame_string() { - return "AIRFISH"; + return "AIRFISH"; //TODO: Change to be able to change with different frame_classes } /* diff --git a/Blimp/wscript b/Blimp/wscript index ea08530213..a45b13a313 100644 --- a/Blimp/wscript +++ b/Blimp/wscript @@ -19,7 +19,7 @@ def build(bld): 'AP_OSD', 'AP_KDECAN', 'AP_Beacon', - 'AP_AdvancedFailsafe', #for some reason compiling GCS_Common.cpp (in libraries) fails if this isn't included... + 'AP_AdvancedFailsafe', # TODO for some reason compiling GCS_Common.cpp (in libraries) fails if this isn't included. ], )