Mirrors_Framework/ardupilot
1
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot.git synced 2026-10-06 19:00:27 +08:00
Code Issues Packages Projects Releases Wiki Activity

Blimp: Code cleanups

Browse Source
This commit is contained in:
Michelle Rossouw
2021-07-06 14:56:02 +10:00
committed by Andrew Tridgell
parent a7ab766fda
commit 2cbcb2be87
17 changed files with 28 additions and 1057 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Blimp/AP_Arming.cpp
+1 -17
View File
@@ -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;
}
Blimp/Blimp.cpp
+1 -43
View File
@@ -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;
}
/*
Blimp/Blimp.h
-74
View File
@@ -40,32 +40,14 @@
// #include <AP_AccelCal/AP_AccelCal.h> // interface and maths for accelerometer calibration
// #include <AP_InertialSensor/AP_InertialSensor.h> // ArduPilot Mega Inertial Sensor (accel & gyro) Library
#include <AP_AHRS/AP_AHRS.h>
// #include <AP_Mission/AP_Mission.h> // Mission command library
// #include <AC_AttitudeControl/AC_AttitudeControl_Multi.h> // Attitude control library
// #include <AC_AttitudeControl/AC_AttitudeControl_Heli.h> // Attitude control library for traditional helicopter
// #include <AC_AttitudeControl/AC_PosControl.h> // Position control library
// #include <AP_Motors/AP_Motors.h> // AP Motors library
#include <AP_Stats/AP_Stats.h> // statistics library
#include <Filter/Filter.h> // Filter library
#include <AP_Airspeed/AP_Airspeed.h> // needed for AHRS build
#include <AP_Vehicle/AP_Vehicle.h> // needed for AHRS build
#include <AP_InertialNav/AP_InertialNav.h> // ArduPilot Mega inertial navigation library
// #include <AC_WPNav/AC_WPNav.h> // Blimp waypoint navigation library
// #include <AC_WPNav/AC_Loiter.h>
// #include <AC_WPNav/AC_Circle.h> // circle navigation library
// #include <AP_Declination/AP_Declination.h> // ArduPilot Mega Declination Helper Library
#include <AP_RCMapper/AP_RCMapper.h> // RC input mapping library
#include <AP_BattMonitor/AP_BattMonitor.h> // Battery monitor library
// #include <AP_LandingGear/AP_LandingGear.h> // Landing Gear library
// #include <AC_InputManager/AC_InputManager.h> // Pilot input handling library
// #include <AC_InputManager/AC_InputManager_Heli.h> // Heli specific pilot input handling library
#include <AP_Arming/AP_Arming.h>
// #include <AP_SmartRTL/AP_SmartRTL.h>
// #include <AP_TempCalibration/AP_TempCalibration.h>
// #include <AC_AutoTune/AC_AutoTune.h>
// #include <AP_Parachute/AP_Parachute.h>
// #include <AC_Sprayer/AC_Sprayer.h>
// #include <AP_ADSB/AP_ADSB.h>
#include <AP_Scripting/AP_Scripting.h>
// 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 <AP_Mount/AP_Mount.h>
// 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;
Blimp/Fins.cpp
+4 -16
View File
@@ -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;
}
};
Blimp/Fins.h
+1 -26
View File
@@ -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()
Blimp/GCS_Mavlink.cpp
-314
View File
File diff suppressed because it is too large Load Diff
Blimp/Log.cpp
-1
View File
@@ -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() {}
Blimp/Parameters.cpp
+6 -152
View File
File diff suppressed because it is too large Load Diff
Blimp/Parameters.h
+4 -224
View File
File diff suppressed because it is too large Load Diff
Blimp/RC_Channel.cpp
-1
View File
@@ -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;
}
Blimp/failsafe.cpp
+1
View File
@@ -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);
Blimp/mode.cpp
+2 -105
View File
@@ -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();
}
Blimp/mode.h
+1 -52
View File
@@ -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
};
Blimp/mode_land.cpp
+2 -4
View File
@@ -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
Blimp/mode_manual.cpp
+2 -12
View File
@@ -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);
}
}
Blimp/radio.cpp
+2 -10
View File
@@ -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
}
Blimp/system.cpp
+1 -6
View File
@@ -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);
}
}
}