Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Blimp: Code cleanups

This commit is contained in:
Michelle Rossouw 2021-06-17 17:03:49 +10:00 committed by Andrew Tridgell
parent a7ab766fda
commit 2cbcb2be87
17 changed files with 28 additions and 1057 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
Blimp/AP_Arming.cpp
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;
}

44
Blimp/Blimp.cpp
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;
}
/*

74
Blimp/Blimp.h
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;

20
Blimp/Fins.cpp
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;
}
};

27
Blimp/Fins.h
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()

314
Blimp/GCS_Mavlink.cpp
View File

@ -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));

1
Blimp/Log.cpp
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() {}

158
Blimp/Parameters.cpp
View File

@ -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()) {

228
Blimp/Parameters.h
View File

@ -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.

1
Blimp/RC_Channel.cpp
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;
}

1
Blimp/failsafe.cpp
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);

107
Blimp/mode.cpp
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();
}

53
Blimp/mode.h
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
};

6
Blimp/mode_land.cpp
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

14
Blimp/mode_manual.cpp
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);
}
}

12
Blimp/radio.cpp
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
}

7
Blimp/system.cpp
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);
}
}
}