diff --git a/Blimp/AP_Arming.cpp b/Blimp/AP_Arming.cpp index bc0366c05b..81d589a4ea 100644 --- a/Blimp/AP_Arming.cpp +++ b/Blimp/AP_Arming.cpp @@ -154,24 +154,8 @@ bool AP_Arming_Blimp::parameter_checks(bool display_failure) 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; - } } - + return true; } diff --git a/Blimp/Blimp.cpp b/Blimp/Blimp.cpp index 9593cb0062..977c9d1058 100644 --- a/Blimp/Blimp.cpp +++ b/Blimp/Blimp.cpp @@ -35,11 +35,7 @@ const AP_Scheduler::Task Blimp::scheduler_tasks[] = { 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), @@ -50,10 +46,6 @@ const AP_Scheduler::Task Blimp::scheduler_tasks[] = { 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 @@ -62,12 +54,9 @@ const AP_Scheduler::Task Blimp::scheduler_tasks[] = { 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 @@ -84,7 +73,7 @@ void Blimp::get_scheduler_tasks(const AP_Scheduler::Task *&tasks, constexpr int8_t Blimp::_failsafe_priorities[4]; -// Main loop - 50hz +// Main loop void Blimp::fast_loop() { // update INS immediately to get current gyro data populated @@ -118,13 +107,6 @@ void Blimp::fast_loop() 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 0.0f; //MIR no idle throttle. -} - // rc_loops - reads user input from transmitter/receiver // called at 100hz void Blimp::rc_loop() @@ -227,8 +209,6 @@ void Blimp::one_hz_loop() 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 @@ -250,7 +230,6 @@ void Blimp::update_altitude() read_barometer(); if (should_log(MASK_LOG_CTUN)) { - Log_Write_Control_Tuning(); #if HAL_GYROFFT_ENABLED gyro_fft.write_log_messages(); #else @@ -259,28 +238,7 @@ void Blimp::update_altitude() } } -// 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; } /* diff --git a/Blimp/Blimp.h b/Blimp/Blimp.h index 37aa935c25..878fbb0490 100644 --- a/Blimp/Blimp.h +++ b/Blimp/Blimp.h @@ -40,32 +40,14 @@ // #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 @@ -74,16 +56,12 @@ #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 @@ -138,7 +116,6 @@ private: AP_Int8 *flight_modes; const uint8_t num_flight_modes = 6; - #if CONFIG_HAL_BOARD == HAL_BOARD_SITL SITL::SITL sitl; #endif @@ -276,7 +253,6 @@ private: uint8_t auto_trim_counter; bool auto_trim_started = false; - // last valid RC input time uint32_t last_radio_update_ms; @@ -322,17 +298,12 @@ private: 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); @@ -344,19 +315,6 @@ private: 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(); @@ -364,16 +322,6 @@ private: 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(); @@ -388,10 +336,6 @@ private: 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); @@ -410,16 +354,11 @@ private: 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(); @@ -453,14 +392,7 @@ private: // // 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; @@ -476,7 +408,6 @@ private: void read_radio(); void set_throttle_and_failsafe(uint16_t throttle_pwm); void set_throttle_zero_flag(int16_t throttle_control); - int16_t get_throttle_mid(void); // sensors.cpp void read_barometer(void); @@ -509,11 +440,6 @@ private: const char* get_frame_string(); void allocate_motors(void); - // 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; diff --git a/Blimp/Fins.cpp b/Blimp/Fins.cpp index 563a5607e4..b11e6dbdcb 100644 --- a/Blimp/Fins.cpp +++ b/Blimp/Fins.cpp @@ -1,6 +1,6 @@ #include "Blimp.h" -// 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. +// This is the scale used for RC inputs so that they can be scaled to the float point values used in the sine wave code. #define FIN_SCALE_MAX 1000 /* @@ -44,8 +44,6 @@ void Fins::setup_fins() 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, @@ -92,7 +90,7 @@ void Fins::output() fabsf(_down_amp_factor[i]*down_out) + fabsf(_yaw_amp_factor[i]*yaw_out); _off[i] = _right_off_factor[i]*right_out + _front_off_factor[i]*front_out + _down_off_factor[i]*down_out + _yaw_off_factor[i]*yaw_out; - _omm[i] = 1; + _freq[i] = 1; _num_added = 0; if (max(0,_right_amp_factor[i]*right_out) > 0.0f) { @@ -119,12 +117,11 @@ void Fins::output() if (turbo_mode) { //double speed fins if offset at max... if (_amp[i] <= 0.6 && fabsf(_off[i]) >= 0.4) { - _omm[i] = 2; + _freq[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]; + _pos[i]= _amp[i]*cosf(freq_hz * _freq[i] * _time * 2 * M_PI) + _off[i]; SRV_Channels::set_output_scaled(SRV_Channels::get_motor_function(i), _pos[i] * FIN_SCALE_MAX); } @@ -141,12 +138,3 @@ void Fins::output_min() yaw_out = 0; Fins::output(); } - -// 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)) { - // 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 index e546f5b4b4..1de1e1c019 100644 --- a/Blimp/Fins.h +++ b/Blimp/Fins.h @@ -26,16 +26,6 @@ public: // var_info for holding Parameter information static const struct AP_Param::GroupInfo var_info[]; - enum class DesiredSpoolState : uint8_t { - SHUT_DOWN = 0, // all fins should move to stop - THROTTLE_UNLIMITED = 2, // all fins can move as needed - }; - - enum class SpoolState : uint8_t { - SHUT_DOWN = 0, // all motors stop - THROTTLE_UNLIMITED = 3, // throttle is no longer constrained by start up procedure - }; - bool initialised_ok() const { return true; @@ -59,8 +49,6 @@ protected: 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 _throttle_avg_max; // last throttle input from set_throttle_avg_max - DesiredSpoolState _spool_desired; // desired spool state - SpoolState _spool_state; // current spool mode float _time; //current timestep @@ -68,7 +56,7 @@ protected: float _amp[NUM_FINS]; //amplitudes float _off[NUM_FINS]; //offsets - float _omm[NUM_FINS]; //omega multiplier + float _freq[NUM_FINS]; //frequency multiplier float _pos[NUM_FINS]; //servo positions float _right_amp_factor[NUM_FINS]; @@ -96,12 +84,6 @@ public: 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 max(float one, float two) { if (one >= two) { @@ -118,13 +100,6 @@ public: void setup_fins(); - float get_throttle_hover() - { - return 0; //TODO - } - - void set_desired_spool_state(DesiredSpoolState spool); - void output(); float get_throttle() diff --git a/Blimp/GCS_Mavlink.cpp b/Blimp/GCS_Mavlink.cpp index abf459cd6d..cd4730607f 100644 --- a/Blimp/GCS_Mavlink.cpp +++ b/Blimp/GCS_Mavlink.cpp @@ -20,37 +20,6 @@ MAV_TYPE GCS_Blimp::frame_type() const 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; // we are armed if we are not initialising @@ -113,49 +82,6 @@ void GCS_MAVLINK_Blimp::send_position_target_global_int() 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_vel_desired_cms() * 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_vel_desired_cms() * 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 { @@ -450,8 +376,6 @@ void GCS_MAVLINK_Blimp::handle_change_alt_request(AP_Mission::Mission_Command &c 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, @@ -570,42 +494,9 @@ MAV_RESULT GCS_MAVLINK_Blimp::handle_command_long_packet(const mavlink_command_l 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] @@ -632,211 +523,6 @@ void GCS_MAVLINK_Blimp::handleMessage(const mavlink_message_t &msg) { switch (msg.msgid) { - // #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)); diff --git a/Blimp/Log.cpp b/Blimp/Log.cpp index a5203afbad..cb0cf62dc9 100644 --- a/Blimp/Log.cpp +++ b/Blimp/Log.cpp @@ -452,7 +452,6 @@ void Blimp::log_init(void) #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() {} diff --git a/Blimp/Parameters.cpp b/Blimp/Parameters.cpp index 770f89d33c..56a1b7b253 100644 --- a/Blimp/Parameters.cpp +++ b/Blimp/Parameters.cpp @@ -89,7 +89,7 @@ const AP_Param::Info Blimp::var_info[] = { // @Units: s // @Range: 0 30 // @Increment: 1 - GSCALAR(telem_delay, "TELEM_DELAY", 0), + GSCALAR(telem_delay, "TELEM_DELAY", 0), // @Param: GCS_PID_MASK // @DisplayName: GCS PID tuning mask @@ -113,49 +113,6 @@ const AP_Param::Info Blimp::var_info[] = { // @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 @@ -271,39 +228,6 @@ const AP_Param::Info Blimp::var_info[] = { // @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 - // @Increment: 10 - // @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 - // @Increment: 10 - // @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 @@ -334,26 +258,8 @@ const AP_Param::Info Blimp::var_info[] = { // @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), @@ -362,26 +268,6 @@ const AP_Param::Info Blimp::var_info[] = { // @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), @@ -453,9 +339,9 @@ const AP_Param::Info Blimp::var_info[] = { GOBJECT(barometer, "BARO", AP_Baro), // GPS driver - // @Group: GPS_ + // @Group: GPS // @Path: ../libraries/AP_GPS/AP_GPS.cpp - GOBJECT(gps, "GPS_", AP_GPS), + GOBJECT(gps, "GPS", AP_GPS), // @Group: SCHED_ // @Path: ../libraries/AP_Scheduler/AP_Scheduler.cpp @@ -505,29 +391,13 @@ const AP_Param::Info Blimp::var_info[] = { */ 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 + // @Bitmask: 0:Unknown // @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 @@ -543,8 +413,8 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = { // @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 + // @Description: Controls major frame class for blimp. + // @Values: 0:Finnedblimp // @User: Standard // @RebootRequired: True AP_GROUPINFO("FRAME_CLASS", 15, ParametersG2, frame_class, DEFAULT_FRAME_CLASS), @@ -557,12 +427,6 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = { // @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 @@ -572,15 +436,6 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = { // @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 @@ -634,7 +489,6 @@ ParametersG2::ParametersG2(void) AP_Param::setup_object_defaults(this, var_info); } - void Blimp::load_parameters(void) { if (!AP_Param::check_var_info()) { diff --git a/Blimp/Parameters.h b/Blimp/Parameters.h index 47c82143ba..cfa9bad239 100644 --- a/Blimp/Parameters.h +++ b/Blimp/Parameters.h @@ -16,7 +16,7 @@ public: // The increment will prevent old parameters from being used incorrectly // by newer code. // - static const uint16_t k_format_version = 120; + static const uint16_t k_format_version = 1; // Parameter identities. // @@ -41,10 +41,7 @@ public: // 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, @@ -60,12 +57,6 @@ public: // scheduler object (for debugging) k_param_scheduler, - // relay object - k_param_relay, - - // (old) EPM object - k_param_epm_unused, - // BoardConfig object k_param_BoardConfig, @@ -82,92 +73,23 @@ public: 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 @@ -183,33 +105,15 @@ public: // 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, @@ -226,33 +130,16 @@ public: // // 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 // @@ -260,42 +147,14 @@ public: // 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 = 170, 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 @@ -312,63 +171,13 @@ public: k_param_initial_mode, // - // 210: Waypoint data + // 220: Misc // - 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; @@ -383,29 +192,9 @@ public: 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; @@ -430,7 +219,6 @@ public: 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; @@ -438,15 +226,7 @@ public: 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; + AP_Int16 rc_speed; // speed of fast RC Channels in Hz // Note: keep initializers here in the same order as they are declared // above. diff --git a/Blimp/RC_Channel.cpp b/Blimp/RC_Channel.cpp index 49102c85e0..31560c4c62 100644 --- a/Blimp/RC_Channel.cpp +++ b/Blimp/RC_Channel.cpp @@ -113,7 +113,6 @@ bool RC_Channel_Blimp::do_aux_function(const aux_func_t ch_option, const AuxSwit default: return RC_Channel::do_aux_function(ch_option, ch_flag); } - return true; } diff --git a/Blimp/failsafe.cpp b/Blimp/failsafe.cpp index adf9fc67dc..0c05d3ea91 100644 --- a/Blimp/failsafe.cpp +++ b/Blimp/failsafe.cpp @@ -56,6 +56,7 @@ void Blimp::failsafe_check() // reduce motors to minimum (we do not immediately disarm because we want to log the failure) if (motors->armed()) { motors->output_min(); + //TODO: this may not work correctly. } AP::logger().Write_Error(LogErrorSubsystem::CPU, LogErrorCode::FAILSAFE_OCCURRED); diff --git a/Blimp/mode.cpp b/Blimp/mode.cpp index 6fd04f6400..19e99553ce 100644 --- a/Blimp/mode.cpp +++ b/Blimp/mode.cpp @@ -44,7 +44,7 @@ Mode *Blimp::mode_from_mode_num(const Mode::Number mode) // 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 +// Some modes 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) { @@ -63,21 +63,6 @@ bool Blimp::set_mode(Mode::Number mode, ModeReason reason) 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()) { @@ -139,16 +124,12 @@ bool Blimp::set_mode(const uint8_t new_mode, const ModeReason reason) // 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) -{ -} + Mode *&new_flightmode){} // notify_flight_mode - sets notify object based on current flight mode. Only used for OreoLED notify device void Blimp::notify_flight_mode() @@ -186,85 +167,6 @@ bool Mode::is_disarmed_or_landed() const 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); - } -} - -/* - 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.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; -} - -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; -} - -// 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); @@ -279,8 +181,3 @@ GCS_Blimp &Mode::gcs() { return blimp.gcs(); } - -uint16_t Mode::get_pilot_speed_dn() -{ - return blimp.get_pilot_speed_dn(); -} diff --git a/Blimp/mode.h b/Blimp/mode.h index 60eda6b77f..8bac2d211c 100644 --- a/Blimp/mode.h +++ b/Blimp/mode.h @@ -13,32 +13,8 @@ public: // Auto Pilot Modes enumeration enum class Number : uint8_t { - MANUAL = 0, // manual control similar to Copter's stabilize mode + MANUAL = 0, // manual control 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 @@ -78,10 +54,6 @@ public: 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; @@ -113,22 +85,8 @@ public: 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_vel_desired_cms() { - // // note that position control isn't used in every mode, so - // // this may return bogus data: - // return pos_control->get_vel_desired_cms(); - // } protected: @@ -137,18 +95,12 @@ protected: // 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; - // convenience references to avoid code churn in conversion: Parameters &g; ParametersG2 &g2; @@ -227,13 +179,10 @@ public: // 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 }; diff --git a/Blimp/mode_land.cpp b/Blimp/mode_land.cpp index f6bb1cbb76..521351311e 100644 --- a/Blimp/mode_land.cpp +++ b/Blimp/mode_land.cpp @@ -1,11 +1,9 @@ #include "Blimp.h" - /* - * Init and run calls for stabilize flight mode + * Init and run calls for land flight mode */ -// manual_run - runs the main manual controller -// should be called at 100hz or more +// Runs the main land controller void ModeLand::run() { //stop moving diff --git a/Blimp/mode_manual.cpp b/Blimp/mode_manual.cpp index d91d60f668..063353ded3 100644 --- a/Blimp/mode_manual.cpp +++ b/Blimp/mode_manual.cpp @@ -1,23 +1,13 @@ #include "Blimp.h" /* - * Init and run calls for stabilize flight mode + * Init and run calls for manual flight mode */ -// manual_run - runs the main manual controller -// should be called at 100hz or more +// Runs the main manual controller void ModeManual::run() { - 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); - } - } diff --git a/Blimp/radio.cpp b/Blimp/radio.cpp index 6081dbdffd..e7640c44c2 100644 --- a/Blimp/radio.cpp +++ b/Blimp/radio.cpp @@ -154,13 +154,5 @@ void Blimp::set_throttle_zero_flag(int16_t throttle_control) } else if (tnow_ms - last_nonzero_throttle_ms > THROTTLE_ZERO_DEBOUNCE_TIME_MS) { ap.throttle_zero = true; } - //MIR What does this mean?? -} - -/* - 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(); -} + //TODO: This may not be needed +} \ No newline at end of file diff --git a/Blimp/system.cpp b/Blimp/system.cpp index 5eea159ab8..5ef5e11aa0 100644 --- a/Blimp/system.cpp +++ b/Blimp/system.cpp @@ -213,15 +213,10 @@ void Blimp::update_auto_armed() set_auto_armed(false); return; } - // if in stabilize or acro flight mode and throttle is zero, auto-armed should become false + // if in a manual 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); - } } }