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

Sub: Refactor "Copter" to "Sub".

This commit is contained in:
Rustom Jehangir 2016-01-14 11:30:56 -08:00 committed by Andrew Tridgell
parent aaf9bec83a
commit 83ff3931b8
61 changed files with 718 additions and 718 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
ArduSub/AP_State.cpp
View File

@ -1,9 +1,9 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#include "Copter.h"
#include "Sub.h"
// set_home_state - update home state
void Copter::set_home_state(enum HomeState new_home_state)
void Sub::set_home_state(enum HomeState new_home_state)
{
// if no change, exit immediately
if (ap.home_state == new_home_state)
@ -19,13 +19,13 @@ void Copter::set_home_state(enum HomeState new_home_state)
}
// home_is_set - returns true if home positions has been set (to GPS location, armed location or EKF origin)
bool Copter::home_is_set()
bool Sub::home_is_set()
{
return (ap.home_state == HOME_SET_NOT_LOCKED || ap.home_state == HOME_SET_AND_LOCKED);
}
// ---------------------------------------------
void Copter::set_auto_armed(bool b)
void Sub::set_auto_armed(bool b)
{
// if no change, exit immediately
if( ap.auto_armed == b )
@ -38,7 +38,7 @@ void Copter::set_auto_armed(bool b)
}
// ---------------------------------------------
void Copter::set_simple_mode(uint8_t b)
void Sub::set_simple_mode(uint8_t b)
{
if(ap.simple_mode != b){
if(b == 0){
@ -58,7 +58,7 @@ void Copter::set_simple_mode(uint8_t b)
}
// ---------------------------------------------
void Copter::set_failsafe_radio(bool b)
void Sub::set_failsafe_radio(bool b)
{
// only act on changes
// -------------------
@ -85,21 +85,21 @@ void Copter::set_failsafe_radio(bool b)
// ---------------------------------------------
void Copter::set_failsafe_battery(bool b)
void Sub::set_failsafe_battery(bool b)
{
failsafe.battery = b;
AP_Notify::flags.failsafe_battery = b;
}
// ---------------------------------------------
void Copter::set_failsafe_gcs(bool b)
void Sub::set_failsafe_gcs(bool b)
{
failsafe.gcs = b;
}
// ---------------------------------------------
void Copter::set_pre_arm_check(bool b)
void Sub::set_pre_arm_check(bool b)
{
if(ap.pre_arm_check != b) {
ap.pre_arm_check = b;
@ -107,14 +107,14 @@ void Copter::set_pre_arm_check(bool b)
}
}
void Copter::set_pre_arm_rc_check(bool b)
void Sub::set_pre_arm_rc_check(bool b)
{
if(ap.pre_arm_rc_check != b) {
ap.pre_arm_rc_check = b;
}
}
void Copter::update_using_interlock()
void Sub::update_using_interlock()
{
#if FRAME_CONFIG == HELI_FRAME
// helicopters are always using motor interlock
@ -125,7 +125,7 @@ void Copter::update_using_interlock()
#endif
}
void Copter::set_motor_emergency_stop(bool b)
void Sub::set_motor_emergency_stop(bool b)
{
if(ap.motor_emergency_stop != b) {
ap.motor_emergency_stop = b;

54
ArduSub/ArduSub.cpp
View File

@ -73,9 +73,9 @@
*
*/
#include "Copter.h"
#include "Sub.h"
#define SCHED_TASK(func, rate_hz, max_time_micros) SCHED_TASK_CLASS(Copter, &copter, func, rate_hz, max_time_micros)
#define SCHED_TASK(func, rate_hz, max_time_micros) SCHED_TASK_CLASS(Sub, &sub, func, rate_hz, max_time_micros)
/*
scheduler table for fast CPUs - all regular tasks apart from the fast_loop()
@ -93,7 +93,7 @@
4000 = 0.1hz
*/
const AP_Scheduler::Task Copter::scheduler_tasks[] = {
const AP_Scheduler::Task Sub::scheduler_tasks[] = {
SCHED_TASK(rc_loop, 100, 130),
SCHED_TASK(throttle_loop, 50, 75),
SCHED_TASK(update_GPS, 50, 200),
@ -163,7 +163,7 @@ const AP_Scheduler::Task Copter::scheduler_tasks[] = {
};
void Copter::setup()
void Sub::setup()
{
cliSerial = hal.console;
@ -186,7 +186,7 @@ void Copter::setup()
/*
if the compass is enabled then try to accumulate a reading
*/
void Copter::compass_accumulate(void)
void Sub::compass_accumulate(void)
{
if (g.compass_enabled) {
compass.accumulate();
@ -196,12 +196,12 @@ void Copter::compass_accumulate(void)
/*
try to accumulate a baro reading
*/
void Copter::barometer_accumulate(void)
void Sub::barometer_accumulate(void)
{
barometer.accumulate();
}
void Copter::perf_update(void)
void Sub::perf_update(void)
{
if (should_log(MASK_LOG_PM))
Log_Write_Performance();
@ -216,7 +216,7 @@ void Copter::perf_update(void)
pmTest1 = 0;
}
void Copter::loop()
void Sub::loop()
{
// wait for an INS sample
ins.wait_for_sample();
@ -251,7 +251,7 @@ void Copter::loop()
// Main loop - 400hz
void Copter::fast_loop()
void Sub::fast_loop()
{
// IMU DCM Algorithm
@ -298,7 +298,7 @@ void Copter::fast_loop()
// rc_loops - reads user input from transmitter/receiver
// called at 100hz
void Copter::rc_loop()
void Sub::rc_loop()
{
// Read radio and 3-position switch on radio
// -----------------------------------------
@ -308,7 +308,7 @@ void Copter::rc_loop()
// throttle_loop - should be run at 50 hz
// ---------------------------
void Copter::throttle_loop()
void Sub::throttle_loop()
{
// get altitude and climb rate from inertial lib
read_inertial_altitude();
@ -334,7 +334,7 @@ void Copter::throttle_loop()
// update_mount - update camera mount position
// should be run at 50hz
void Copter::update_mount()
void Sub::update_mount()
{
#if MOUNT == ENABLED
// update camera mount's position
@ -348,7 +348,7 @@ void Copter::update_mount()
// update_batt_compass - read battery and compass
// should be called at 10hz
void Copter::update_batt_compass(void)
void Sub::update_batt_compass(void)
{
// read battery before compass because it may be used for motor interference compensation
read_battery();
@ -366,7 +366,7 @@ void Copter::update_batt_compass(void)
// ten_hz_logging_loop
// should be run at 10hz
void Copter::ten_hz_logging_loop()
void Sub::ten_hz_logging_loop()
{
// log attitude data if we're not already logging at the higher rate
if (should_log(MASK_LOG_ATTITUDE_MED) && !should_log(MASK_LOG_ATTITUDE_FAST)) {
@ -404,7 +404,7 @@ void Copter::ten_hz_logging_loop()
// fifty_hz_logging_loop
// should be run at 50hz
void Copter::fifty_hz_logging_loop()
void Sub::fifty_hz_logging_loop()
{
#if HIL_MODE != HIL_MODE_DISABLED
// HIL for a copter needs very fast update of the servo values
@ -432,7 +432,7 @@ void Copter::fifty_hz_logging_loop()
// full_rate_logging_loop
// should be run at the MAIN_LOOP_RATE
void Copter::full_rate_logging_loop()
void Sub::full_rate_logging_loop()
{
if (should_log(MASK_LOG_IMU_FAST) && !should_log(MASK_LOG_IMU_RAW)) {
DataFlash.Log_Write_IMU(ins);
@ -442,13 +442,13 @@ void Copter::full_rate_logging_loop()
}
}
void Copter::dataflash_periodic(void)
void Sub::dataflash_periodic(void)
{
DataFlash.periodic_tasks();
}
// three_hz_loop - 3.3hz loop
void Copter::three_hz_loop()
void Sub::three_hz_loop()
{
// check if we've lost contact with the ground station
failsafe_gcs_check();
@ -469,7 +469,7 @@ void Copter::three_hz_loop()
}
// one_hz_loop - runs at 1Hz
void Copter::one_hz_loop()
void Sub::one_hz_loop()
{
if (should_log(MASK_LOG_ANY)) {
Log_Write_Data(DATA_AP_STATE, ap.value);
@ -520,7 +520,7 @@ void Copter::one_hz_loop()
}
// called at 50hz
void Copter::update_GPS(void)
void Sub::update_GPS(void)
{
static uint32_t last_gps_reading[GPS_MAX_INSTANCES]; // time of last gps message
bool gps_updated = false;
@ -549,7 +549,7 @@ void Copter::update_GPS(void)
if (gps.status() >= AP_GPS::GPS_OK_FIX_3D) {
#if CAMERA == ENABLED
if (camera.update_location(current_loc, copter.ahrs) == true) {
if (camera.update_location(current_loc, sub.ahrs) == true) {
do_take_picture();
}
#endif
@ -557,7 +557,7 @@ void Copter::update_GPS(void)
}
}
void Copter::init_simple_bearing()
void Sub::init_simple_bearing()
{
// capture current cos_yaw and sin_yaw values
simple_cos_yaw = ahrs.cos_yaw();
@ -575,7 +575,7 @@ void Copter::init_simple_bearing()
}
// update_simple_mode - rotates pilot input if we are in simple mode
void Copter::update_simple_mode(void)
void Sub::update_simple_mode(void)
{
float rollx, pitchx;
@ -604,7 +604,7 @@ void Copter::update_simple_mode(void)
// update_super_simple_bearing - adjusts simple bearing based on location
// should be called after home_bearing has been updated
void Copter::update_super_simple_bearing(bool force_update)
void Sub::update_super_simple_bearing(bool force_update)
{
// check if we are in super simple mode and at least 10m from home
if(force_update || (ap.simple_mode == 2 && home_distance > SUPER_SIMPLE_RADIUS)) {
@ -618,7 +618,7 @@ void Copter::update_super_simple_bearing(bool force_update)
}
}
void Copter::read_AHRS(void)
void Sub::read_AHRS(void)
{
// Perform IMU calculations and get attitude info
//-----------------------------------------------
@ -631,7 +631,7 @@ void Copter::read_AHRS(void)
}
// read baro and sonar altitude at 10hz
void Copter::update_altitude()
void Sub::update_altitude()
{
// read in baro altitude
read_barometer();
@ -645,4 +645,4 @@ void Copter::update_altitude()
}
}
AP_HAL_MAIN_CALLBACKS(&copter);
AP_HAL_MAIN_CALLBACKS(&sub);

36
ArduSub/Attitude.cpp
View File

@ -1,17 +1,17 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#include "Copter.h"
#include "Sub.h"
// get_smoothing_gain - returns smoothing gain to be passed into attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth
// result is a number from 2 to 12 with 2 being very sluggish and 12 being very crisp
float Copter::get_smoothing_gain()
float Sub::get_smoothing_gain()
{
return (2.0f + (float)g.rc_feel_rp/10.0f);
}
// get_pilot_desired_angle - transform pilot's roll or pitch input into a desired lean angle
// returns desired angle in centi-degrees
void Copter::get_pilot_desired_lean_angles(float roll_in, float pitch_in, float &roll_out, float &pitch_out, float angle_max)
void Sub::get_pilot_desired_lean_angles(float roll_in, float pitch_in, float &roll_out, float &pitch_out, float angle_max)
{
// sanity check angle max parameter
aparm.angle_max = constrain_int16(aparm.angle_max,1000,8000);
@ -43,14 +43,14 @@ void Copter::get_pilot_desired_lean_angles(float roll_in, float pitch_in, float
// get_pilot_desired_heading - transform pilot's yaw input into a
// desired yaw rate
// returns desired yaw rate in centi-degrees per second
float Copter::get_pilot_desired_yaw_rate(int16_t stick_angle)
float Sub::get_pilot_desired_yaw_rate(int16_t stick_angle)
{
// convert pilot input to the desired yaw rate
return stick_angle * g.acro_yaw_p;
}
// check for ekf yaw reset and adjust target heading
void Copter::check_ekf_yaw_reset()
void Sub::check_ekf_yaw_reset()
{
float yaw_angle_change_rad = 0.0f;
uint32_t new_ekfYawReset_ms = ahrs.getLastYawResetAngle(yaw_angle_change_rad);
@ -66,7 +66,7 @@ void Copter::check_ekf_yaw_reset()
// get_roi_yaw - returns heading towards location held in roi_WP
// should be called at 100hz
float Copter::get_roi_yaw()
float Sub::get_roi_yaw()
{
static uint8_t roi_yaw_counter = 0; // used to reduce update rate to 100hz
@ -79,7 +79,7 @@ float Copter::get_roi_yaw()
return yaw_look_at_WP_bearing;
}
float Copter::get_look_ahead_yaw()
float Sub::get_look_ahead_yaw()
{
const Vector3f& vel = inertial_nav.get_velocity();
float speed = pythagorous2(vel.x,vel.y);
@ -96,7 +96,7 @@ float Copter::get_look_ahead_yaw()
// update_thr_average - update estimated throttle required to hover (if necessary)
// should be called at 100hz
void Copter::update_thr_average()
void Sub::update_thr_average()
{
// ensure throttle_average has been initialised
if( is_zero(throttle_average) ) {
@ -122,7 +122,7 @@ void Copter::update_thr_average()
}
// set_throttle_takeoff - allows parents to tell throttle controller we are taking off so I terms can be cleared
void Copter::set_throttle_takeoff()
void Sub::set_throttle_takeoff()
{
// tell position controller to reset alt target and reset I terms
pos_control.init_takeoff();
@ -134,7 +134,7 @@ void Copter::set_throttle_takeoff()
// get_pilot_desired_throttle - transform pilot's throttle input to make cruise throttle mid stick
// used only for manual throttle modes
// returns throttle output 0 to 1000
int16_t Copter::get_pilot_desired_throttle(int16_t throttle_control)
int16_t Sub::get_pilot_desired_throttle(int16_t throttle_control)
{
int16_t throttle_out;
@ -162,7 +162,7 @@ int16_t Copter::get_pilot_desired_throttle(int16_t throttle_control)
// get_pilot_desired_climb_rate - transform pilot's throttle input to
// climb rate in cm/s. we use radio_in instead of control_in to get the full range
// without any deadzone at the bottom
float Copter::get_pilot_desired_climb_rate(float throttle_control)
float Sub::get_pilot_desired_climb_rate(float throttle_control)
{
// throttle failsafe check
if( failsafe.radio ) {
@ -199,12 +199,12 @@ float Copter::get_pilot_desired_climb_rate(float throttle_control)
}
// get_non_takeoff_throttle - a throttle somewhere between min and mid throttle which should not lead to a takeoff
float Copter::get_non_takeoff_throttle()
float Sub::get_non_takeoff_throttle()
{
return (g.throttle_mid / 2.0f);
}
float Copter::get_takeoff_trigger_throttle()
float Sub::get_takeoff_trigger_throttle()
{
return channel_throttle->get_control_mid() + g.takeoff_trigger_dz;
}
@ -212,7 +212,7 @@ float Copter::get_takeoff_trigger_throttle()
// get_throttle_pre_takeoff - convert pilot's input throttle to a throttle output before take-off
// used only for althold, loiter, hybrid flight modes
// returns throttle output 0 to 1000
float Copter::get_throttle_pre_takeoff(float input_thr)
float Sub::get_throttle_pre_takeoff(float input_thr)
{
// exit immediately if input_thr is zero
if (input_thr <= 0.0f) {
@ -251,7 +251,7 @@ float Copter::get_throttle_pre_takeoff(float input_thr)
// get_surface_tracking_climb_rate - hold copter at the desired distance above the ground
// returns climb rate (in cm/s) which should be passed to the position controller
float Copter::get_surface_tracking_climb_rate(int16_t target_rate, float current_alt_target, float dt)
float Sub::get_surface_tracking_climb_rate(int16_t target_rate, float current_alt_target, float dt)
{
static uint32_t last_call_ms = 0;
float distance_error;
@ -285,14 +285,14 @@ float Copter::get_surface_tracking_climb_rate(int16_t target_rate, float current
}
// set_accel_throttle_I_from_pilot_throttle - smoothes transition from pilot controlled throttle to autopilot throttle
void Copter::set_accel_throttle_I_from_pilot_throttle(int16_t pilot_throttle)
void Sub::set_accel_throttle_I_from_pilot_throttle(int16_t pilot_throttle)
{
// shift difference between pilot's throttle and hover throttle into accelerometer I
g.pid_accel_z.set_integrator(pilot_throttle-throttle_average);
}
// updates position controller's maximum altitude using fence and EKF limits
void Copter::update_poscon_alt_max()
void Sub::update_poscon_alt_max()
{
float alt_limit_cm = 0.0f; // interpreted as no limit if left as zero
@ -316,7 +316,7 @@ void Copter::update_poscon_alt_max()
}
// rotate vector from vehicle's perspective to North-East frame
void Copter::rotate_body_frame_to_NE(float &x, float &y)
void Sub::rotate_body_frame_to_NE(float &x, float &y)
{
float ne_x = x*ahrs.cos_yaw() - y*ahrs.sin_yaw();
float ne_y = x*ahrs.sin_yaw() + y*ahrs.cos_yaw();

414
ArduSub/GCS_Mavlink.cpp
View File

File diff suppressed because it is too large Load Diff

142
ArduSub/Log.cpp
View File

@ -1,6 +1,6 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#include "Copter.h"
#include "Sub.h"
#if LOGGING_ENABLED == ENABLED
@ -21,9 +21,9 @@ static const struct Menu::command log_menu_commands[] = {
};
// A Macro to create the Menu
MENU2(log_menu, "Log", log_menu_commands, FUNCTOR_BIND(&copter, &Copter::print_log_menu, bool));
MENU2(log_menu, "Log", log_menu_commands, FUNCTOR_BIND(&sub, &Sub::print_log_menu, bool));
bool Copter::print_log_menu(void)
bool Sub::print_log_menu(void)
{
cliSerial->printf("logs enabled: ");
@ -55,7 +55,7 @@ bool Copter::print_log_menu(void)
}
#if CLI_ENABLED == ENABLED
int8_t Copter::dump_log(uint8_t argc, const Menu::arg *argv)
int8_t Sub::dump_log(uint8_t argc, const Menu::arg *argv)
{
int16_t dump_log_num;
uint16_t dump_log_start;
@ -82,7 +82,7 @@ int8_t Copter::dump_log(uint8_t argc, const Menu::arg *argv)
}
#endif
int8_t Copter::erase_logs(uint8_t argc, const Menu::arg *argv)
int8_t Sub::erase_logs(uint8_t argc, const Menu::arg *argv)
{
in_mavlink_delay = true;
do_erase_logs();
@ -90,7 +90,7 @@ int8_t Copter::erase_logs(uint8_t argc, const Menu::arg *argv)
return 0;
}
int8_t Copter::select_logs(uint8_t argc, const Menu::arg *argv)
int8_t Sub::select_logs(uint8_t argc, const Menu::arg *argv)
{
uint16_t bits;
@ -138,14 +138,14 @@ int8_t Copter::select_logs(uint8_t argc, const Menu::arg *argv)
return(0);
}
int8_t Copter::process_logs(uint8_t argc, const Menu::arg *argv)
int8_t Sub::process_logs(uint8_t argc, const Menu::arg *argv)
{
log_menu.run();
return 0;
}
#endif // CLI_ENABLED
void Copter::do_erase_logs(void)
void Sub::do_erase_logs(void)
{
gcs_send_text(MAV_SEVERITY_INFO, "Erasing logs");
DataFlash.EraseAll();
@ -168,7 +168,7 @@ struct PACKED log_AutoTune {
};
// Write an Autotune data packet
void Copter::Log_Write_AutoTune(uint8_t axis, uint8_t tune_step, float meas_target, float meas_min, float meas_max, float new_gain_rp, float new_gain_rd, float new_gain_sp, float new_ddt)
void Sub::Log_Write_AutoTune(uint8_t axis, uint8_t tune_step, float meas_target, float meas_min, float meas_max, float new_gain_rp, float new_gain_rd, float new_gain_sp, float new_ddt)
{
struct log_AutoTune pkt = {
LOG_PACKET_HEADER_INIT(LOG_AUTOTUNE_MSG),
@ -194,7 +194,7 @@ struct PACKED log_AutoTuneDetails {
};
// Write an Autotune data packet
void Copter::Log_Write_AutoTuneDetails(float angle_cd, float rate_cds)
void Sub::Log_Write_AutoTuneDetails(float angle_cd, float rate_cds)
{
struct log_AutoTuneDetails pkt = {
LOG_PACKET_HEADER_INIT(LOG_AUTOTUNEDETAILS_MSG),
@ -207,7 +207,7 @@ void Copter::Log_Write_AutoTuneDetails(float angle_cd, float rate_cds)
#endif
// Write a Current data packet
void Copter::Log_Write_Current()
void Sub::Log_Write_Current()
{
DataFlash.Log_Write_Current(battery, (int16_t)(motors.get_throttle()));
@ -226,7 +226,7 @@ struct PACKED log_Optflow {
};
// Write an optical flow packet
void Copter::Log_Write_Optflow()
void Sub::Log_Write_Optflow()
{
#if OPTFLOW == ENABLED
// exit immediately if not enabled
@ -264,7 +264,7 @@ struct PACKED log_Nav_Tuning {
};
// Write an Nav Tuning packet
void Copter::Log_Write_Nav_Tuning()
void Sub::Log_Write_Nav_Tuning()
{
const Vector3f &pos_target = pos_control.get_pos_target();
const Vector3f &vel_target = pos_control.get_vel_target();
@ -305,7 +305,7 @@ struct PACKED log_Control_Tuning {
};
// Write a control tuning packet
void Copter::Log_Write_Control_Tuning()
void Sub::Log_Write_Control_Tuning()
{
struct log_Control_Tuning pkt = {
LOG_PACKET_HEADER_INIT(LOG_CONTROL_TUNING_MSG),
@ -336,7 +336,7 @@ struct PACKED log_Performance {
};
// Write a performance monitoring packet
void Copter::Log_Write_Performance()
void Sub::Log_Write_Performance()
{
struct log_Performance pkt = {
LOG_PACKET_HEADER_INIT(LOG_PERFORMANCE_MSG),
@ -352,7 +352,7 @@ void Copter::Log_Write_Performance()
}
// Write an attitude packet
void Copter::Log_Write_Attitude()
void Sub::Log_Write_Attitude()
{
Vector3f targets = attitude_control.get_att_target_euler_cd();
targets.z = wrap_360_cd_float(targets.z);
@ -388,7 +388,7 @@ struct PACKED log_Rate {
};
// Write an rate packet
void Copter::Log_Write_Rate()
void Sub::Log_Write_Rate()
{
const Vector3f &rate_targets = attitude_control.rate_bf_targets();
const Vector3f &accel_target = pos_control.get_accel_target();
@ -421,7 +421,7 @@ struct PACKED log_MotBatt {
};
// Write an rate packet
void Copter::Log_Write_MotBatt()
void Sub::Log_Write_MotBatt()
{
#if FRAME_CONFIG != HELI_FRAME
struct log_MotBatt pkt_mot = {
@ -442,7 +442,7 @@ struct PACKED log_Startup {
};
// Write Startup packet
void Copter::Log_Write_Startup()
void Sub::Log_Write_Startup()
{
struct log_Startup pkt = {
LOG_PACKET_HEADER_INIT(LOG_STARTUP_MSG),
@ -458,7 +458,7 @@ struct PACKED log_Event {
};
// Wrote an event packet
void Copter::Log_Write_Event(uint8_t id)
void Sub::Log_Write_Event(uint8_t id)
{
if (should_log(MASK_LOG_ANY)) {
struct log_Event pkt = {
@ -479,7 +479,7 @@ struct PACKED log_Data_Int16t {
// Write an int16_t data packet
UNUSED_FUNCTION
void Copter::Log_Write_Data(uint8_t id, int16_t value)
void Sub::Log_Write_Data(uint8_t id, int16_t value)
{
if (should_log(MASK_LOG_ANY)) {
struct log_Data_Int16t pkt = {
@ -501,7 +501,7 @@ struct PACKED log_Data_UInt16t {
// Write an uint16_t data packet
UNUSED_FUNCTION
void Copter::Log_Write_Data(uint8_t id, uint16_t value)
void Sub::Log_Write_Data(uint8_t id, uint16_t value)
{
if (should_log(MASK_LOG_ANY)) {
struct log_Data_UInt16t pkt = {
@ -522,7 +522,7 @@ struct PACKED log_Data_Int32t {
};
// Write an int32_t data packet
void Copter::Log_Write_Data(uint8_t id, int32_t value)
void Sub::Log_Write_Data(uint8_t id, int32_t value)
{
if (should_log(MASK_LOG_ANY)) {
struct log_Data_Int32t pkt = {
@ -543,7 +543,7 @@ struct PACKED log_Data_UInt32t {
};
// Write a uint32_t data packet
void Copter::Log_Write_Data(uint8_t id, uint32_t value)
void Sub::Log_Write_Data(uint8_t id, uint32_t value)
{
if (should_log(MASK_LOG_ANY)) {
struct log_Data_UInt32t pkt = {
@ -565,7 +565,7 @@ struct PACKED log_Data_Float {
// Write a float data packet
UNUSED_FUNCTION
void Copter::Log_Write_Data(uint8_t id, float value)
void Sub::Log_Write_Data(uint8_t id, float value)
{
if (should_log(MASK_LOG_ANY)) {
struct log_Data_Float pkt = {
@ -586,7 +586,7 @@ struct PACKED log_Error {
};
// Write an error packet
void Copter::Log_Write_Error(uint8_t sub_system, uint8_t error_code)
void Sub::Log_Write_Error(uint8_t sub_system, uint8_t error_code)
{
struct log_Error pkt = {
LOG_PACKET_HEADER_INIT(LOG_ERROR_MSG),
@ -597,7 +597,7 @@ void Copter::Log_Write_Error(uint8_t sub_system, uint8_t error_code)
DataFlash.WriteBlock(&pkt, sizeof(pkt));
}
void Copter::Log_Write_Baro(void)
void Sub::Log_Write_Baro(void)
{
DataFlash.Log_Write_Baro(barometer);
}
@ -612,7 +612,7 @@ struct PACKED log_ParameterTuning {
int16_t tuning_high; // tuning high end value
};
void Copter::Log_Write_Parameter_Tuning(uint8_t param, float tuning_val, int16_t control_in, int16_t tune_low, int16_t tune_high)
void Sub::Log_Write_Parameter_Tuning(uint8_t param, float tuning_val, int16_t control_in, int16_t tune_low, int16_t tune_high)
{
struct log_ParameterTuning pkt_tune = {
LOG_PACKET_HEADER_INIT(LOG_PARAMTUNE_MSG),
@ -628,7 +628,7 @@ void Copter::Log_Write_Parameter_Tuning(uint8_t param, float tuning_val, int16_t
}
// log EKF origin and ahrs home to dataflash
void Copter::Log_Write_Home_And_Origin()
void Sub::Log_Write_Home_And_Origin()
{
// log ekf origin if set
Location ekf_orig;
@ -643,7 +643,7 @@ void Copter::Log_Write_Home_And_Origin()
}
// logs when baro or compass becomes unhealthy
void Copter::Log_Sensor_Health()
void Sub::Log_Sensor_Health()
{
// check baro
if (sensor_health.baro != barometer.healthy()) {
@ -667,7 +667,7 @@ struct PACKED log_Heli {
#if FRAME_CONFIG == HELI_FRAME
// Write an helicopter packet
void Copter::Log_Write_Heli()
void Sub::Log_Write_Heli()
{
struct log_Heli pkt_heli = {
LOG_PACKET_HEADER_INIT(LOG_HELI_MSG),
@ -693,7 +693,7 @@ struct PACKED log_Precland {
};
// Write an optical flow packet
void Copter::Log_Write_Precland()
void Sub::Log_Write_Precland()
{
#if PRECISION_LANDING == ENABLED
// exit immediately if not enabled
@ -719,7 +719,7 @@ void Copter::Log_Write_Precland()
#endif // PRECISION_LANDING == ENABLED
}
const struct LogStructure Copter::log_structure[] = {
const struct LogStructure Sub::log_structure[] = {
LOG_COMMON_STRUCTURES,
#if AUTOTUNE_ENABLED == ENABLED
{ LOG_AUTOTUNE_MSG, sizeof(log_AutoTune),
@ -765,7 +765,7 @@ const struct LogStructure Copter::log_structure[] = {
#if CLI_ENABLED == ENABLED
// Read the DataFlash log memory
void Copter::Log_Read(uint16_t list_entry, uint16_t start_page, uint16_t end_page)
void Sub::Log_Read(uint16_t list_entry, uint16_t start_page, uint16_t end_page)
{
cliSerial->printf("\n" FIRMWARE_STRING
"\nFree RAM: %u\n"
@ -775,12 +775,12 @@ void Copter::Log_Read(uint16_t list_entry, uint16_t start_page, uint16_t end_pag
cliSerial->println(HAL_BOARD_NAME);
DataFlash.LogReadProcess(list_entry, start_page, end_page,
FUNCTOR_BIND_MEMBER(&Copter::print_flight_mode, void, AP_HAL::BetterStream *, uint8_t),
FUNCTOR_BIND_MEMBER(&Sub::print_flight_mode, void, AP_HAL::BetterStream *, uint8_t),
cliSerial);
}
#endif // CLI_ENABLED
void Copter::Log_Write_Vehicle_Startup_Messages()
void Sub::Log_Write_Vehicle_Startup_Messages()
{
// only 200(?) bytes are guaranteed by DataFlash
DataFlash.Log_Write_Message("Frame: " FRAME_CONFIG_STRING);
@ -789,13 +789,13 @@ void Copter::Log_Write_Vehicle_Startup_Messages()
// start a new log
void Copter::start_logging()
void Sub::start_logging()
{
if (g.log_bitmask != 0) {
if (!ap.logging_started) {
ap.logging_started = true;
DataFlash.set_mission(&mission);
DataFlash.setVehicle_Startup_Log_Writer(FUNCTOR_BIND(&copter, &Copter::Log_Write_Vehicle_Startup_Messages, void));
DataFlash.setVehicle_Startup_Log_Writer(FUNCTOR_BIND(&sub, &Sub::Log_Write_Vehicle_Startup_Messages, void));
DataFlash.StartNewLog();
}
// enable writes
@ -803,7 +803,7 @@ void Copter::start_logging()
}
}
void Copter::log_init(void)
void Sub::log_init(void)
{
DataFlash.Init(log_structure, ARRAY_SIZE(log_structure));
if (!DataFlash.CardInserted()) {
@ -822,48 +822,48 @@ void Copter::log_init(void)
#else // LOGGING_ENABLED
#if CLI_ENABLED == ENABLED
bool Copter::print_log_menu(void) { return true; }
int8_t Copter::dump_log(uint8_t argc, const Menu::arg *argv) { return 0; }
int8_t Copter::erase_logs(uint8_t argc, const Menu::arg *argv) { return 0; }
int8_t Copter::select_logs(uint8_t argc, const Menu::arg *argv) { return 0; }
int8_t Copter::process_logs(uint8_t argc, const Menu::arg *argv) { return 0; }
void Copter::Log_Read(uint16_t log_num, uint16_t start_page, uint16_t end_page) {}
bool Sub::print_log_menu(void) { return true; }
int8_t Sub::dump_log(uint8_t argc, const Menu::arg *argv) { return 0; }
int8_t Sub::erase_logs(uint8_t argc, const Menu::arg *argv) { return 0; }
int8_t Sub::select_logs(uint8_t argc, const Menu::arg *argv) { return 0; }
int8_t Sub::process_logs(uint8_t argc, const Menu::arg *argv) { return 0; }
void Sub::Log_Read(uint16_t log_num, uint16_t start_page, uint16_t end_page) {}
#endif // CLI_ENABLED == ENABLED
void Copter::do_erase_logs(void) {}
void Copter::Log_Write_AutoTune(uint8_t axis, uint8_t tune_step, float meas_target, \
void Sub::do_erase_logs(void) {}
void Sub::Log_Write_AutoTune(uint8_t axis, uint8_t tune_step, float meas_target, \
float meas_min, float meas_max, float new_gain_rp, \
float new_gain_rd, float new_gain_sp, float new_ddt) {}
void Copter::Log_Write_AutoTuneDetails(float angle_cd, float rate_cds) {}
void Copter::Log_Write_Current() {}
void Copter::Log_Write_Nav_Tuning() {}
void Copter::Log_Write_Control_Tuning() {}
void Copter::Log_Write_Performance() {}
void Copter::Log_Write_Attitude(void) {}
void Copter::Log_Write_Rate() {}
void Copter::Log_Write_MotBatt() {}
void Copter::Log_Write_Startup() {}
void Copter::Log_Write_Event(uint8_t id) {}
void Copter::Log_Write_Data(uint8_t id, int32_t value) {}
void Copter::Log_Write_Data(uint8_t id, uint32_t value) {}
void Copter::Log_Write_Data(uint8_t id, int16_t value) {}
void Copter::Log_Write_Data(uint8_t id, uint16_t value) {}
void Copter::Log_Write_Data(uint8_t id, float value) {}
void Copter::Log_Write_Error(uint8_t sub_system, uint8_t error_code) {}
void Copter::Log_Write_Baro(void) {}
void Copter::Log_Write_Parameter_Tuning(uint8_t param, float tuning_val, int16_t control_in, int16_t tune_low, int16_t tune_high) {}
void Copter::Log_Write_Home_And_Origin() {}
void Copter::Log_Sensor_Health() {}
void Sub::Log_Write_AutoTuneDetails(float angle_cd, float rate_cds) {}
void Sub::Log_Write_Current() {}
void Sub::Log_Write_Nav_Tuning() {}
void Sub::Log_Write_Control_Tuning() {}
void Sub::Log_Write_Performance() {}
void Sub::Log_Write_Attitude(void) {}
void Sub::Log_Write_Rate() {}
void Sub::Log_Write_MotBatt() {}
void Sub::Log_Write_Startup() {}
void Sub::Log_Write_Event(uint8_t id) {}
void Sub::Log_Write_Data(uint8_t id, int32_t value) {}
void Sub::Log_Write_Data(uint8_t id, uint32_t value) {}
void Sub::Log_Write_Data(uint8_t id, int16_t value) {}
void Sub::Log_Write_Data(uint8_t id, uint16_t value) {}
void Sub::Log_Write_Data(uint8_t id, float value) {}
void Sub::Log_Write_Error(uint8_t sub_system, uint8_t error_code) {}
void Sub::Log_Write_Baro(void) {}
void Sub::Log_Write_Parameter_Tuning(uint8_t param, float tuning_val, int16_t control_in, int16_t tune_low, int16_t tune_high) {}
void Sub::Log_Write_Home_And_Origin() {}
void Sub::Log_Sensor_Health() {}
#if FRAME_CONFIG == HELI_FRAME
void Copter::Log_Write_Heli() {}
void Sub::Log_Write_Heli() {}
#endif
#if OPTFLOW == ENABLED
void Copter::Log_Write_Optflow() {}
void Sub::Log_Write_Optflow() {}
#endif
void Copter::start_logging() {}
void Copter::log_init(void) {}
void Sub::start_logging() {}
void Sub::log_init(void) {}
#endif // LOGGING_ENABLED

16
ArduSub/Parameters.cpp
View File

@ -1,6 +1,6 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#include "Copter.h"
#include "Sub.h"
/*
This program is free software: you can redistribute it and/or modify
@ -22,13 +22,13 @@
*
*/
#define GSCALAR(v, name, def) { copter.g.v.vtype, name, Parameters::k_param_ ## v, &copter.g.v, {def_value : def} }
#define ASCALAR(v, name, def) { copter.aparm.v.vtype, name, Parameters::k_param_ ## v, (const void *)&copter.aparm.v, {def_value : def} }
#define GGROUP(v, name, class) { AP_PARAM_GROUP, name, Parameters::k_param_ ## v, &copter.g.v, {group_info : class::var_info} }
#define GOBJECT(v, name, class) { AP_PARAM_GROUP, name, Parameters::k_param_ ## v, (const void *)&copter.v, {group_info : class::var_info} }
#define GOBJECTN(v, pname, name, class) { AP_PARAM_GROUP, name, Parameters::k_param_ ## pname, (const void *)&copter.v, {group_info : class::var_info} }
#define GSCALAR(v, name, def) { sub.g.v.vtype, name, Parameters::k_param_ ## v, &sub.g.v, {def_value : def} }
#define ASCALAR(v, name, def) { sub.aparm.v.vtype, name, Parameters::k_param_ ## v, (const void *)&sub.aparm.v, {def_value : def} }
#define GGROUP(v, name, class) { AP_PARAM_GROUP, name, Parameters::k_param_ ## v, &sub.g.v, {group_info : class::var_info} }
#define GOBJECT(v, name, class) { AP_PARAM_GROUP, name, Parameters::k_param_ ## v, (const void *)&sub.v, {group_info : class::var_info} }
#define GOBJECTN(v, pname, name, class) { AP_PARAM_GROUP, name, Parameters::k_param_ ## pname, (const void *)&sub.v, {group_info : class::var_info} }
const AP_Param::Info Copter::var_info[] = {
const AP_Param::Info Sub::var_info[] = {
// @Param: SYSID_SW_MREV
// @DisplayName: Eeprom format version number
// @Description: This value is incremented when changes are made to the eeprom format
@ -1146,7 +1146,7 @@ const AP_Param::ConversionInfo conversion_table[] = {
{ Parameters::k_param_serial2_baud, 0, AP_PARAM_INT16, "SERIAL2_BAUD" },
};
void Copter::load_parameters(void)
void Sub::load_parameters(void)
{
if (!AP_Param::check_var_info()) {
cliSerial->printf("Bad var table\n");

14
ArduSub/Copter.cpp → ArduSub/Sub.cpp
View File

@ -1,6 +1,6 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#include "Copter.h"
#include "Sub.h"
/*
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
@ -16,18 +16,18 @@
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
/*
constructor for main Copter class
constructor for main Sub class
*/
const AP_HAL::HAL& hal = AP_HAL::get_HAL();
Copter::Copter(void) :
Sub::Sub(void) :
flight_modes(&g.flight_mode1),
sonar_enabled(true),
mission(ahrs,
FUNCTOR_BIND_MEMBER(&Copter::start_command, bool, const AP_Mission::Mission_Command &),
FUNCTOR_BIND_MEMBER(&Copter::verify_command_callback, bool, const AP_Mission::Mission_Command &),
FUNCTOR_BIND_MEMBER(&Copter::exit_mission, void)),
FUNCTOR_BIND_MEMBER(&Sub::start_command, bool, const AP_Mission::Mission_Command &),
FUNCTOR_BIND_MEMBER(&Sub::verify_command_callback, bool, const AP_Mission::Mission_Command &),
FUNCTOR_BIND_MEMBER(&Sub::exit_mission, void)),
control_mode(STABILIZE),
#if FRAME_CONFIG == HELI_FRAME // helicopter constructor requires more arguments
motors(g.rc_7, g.heli_servo_rsc, g.heli_servo_1, g.heli_servo_2, g.heli_servo_3, g.heli_servo_4, MAIN_LOOP_RATE),
@ -132,4 +132,4 @@ Copter::Copter(void) :
sensor_health.compass = true;
}
Copter copter;
Sub sub;

30
ArduSub/Copter.h → ArduSub/Sub.h
View File

@ -1,9 +1,9 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#ifndef _COPTER_H
#define _COPTER_H
#ifndef _SUB_H
#define _SUB_H
#define THISFIRMWARE "APM:Copter V3.4-dev"
#define THISFIRMWARE "ArduSub V3.4-dev"
#define FIRMWARE_VERSION 3,4,0,FIRMWARE_VERSION_TYPE_DEV
/*
@ -21,7 +21,7 @@
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
/*
This is the main Copter class
This is the main Sub class
*/
////////////////////////////////////////////////////////////////////////////////
@ -81,7 +81,7 @@
#include <AC_WPNav/AC_WPNav.h> // ArduCopter waypoint navigation library
#include <AC_WPNav/AC_Circle.h> // circle navigation library
#include <AP_Declination/AP_Declination.h> // ArduPilot Mega Declination Helper Library
#include <AC_Fence/AC_Fence.h> // Arducopter Fence library
#include <AC_Fence/AC_Fence.h> // ArduCopter Fence library
#include <AP_Scheduler/AP_Scheduler.h> // main loop scheduler
#include <AP_RCMapper/AP_RCMapper.h> // RC input mapping library
#include <AP_Notify/AP_Notify.h> // Notify library
@ -122,12 +122,12 @@
#include <SITL/SITL.h>
#endif
class Copter : public AP_HAL::HAL::Callbacks {
class Sub : public AP_HAL::HAL::Callbacks {
public:
friend class GCS_MAVLINK;
friend class Parameters;
Copter(void);
Sub(void);
// HAL::Callbacks implementation.
void setup() override;
@ -333,7 +333,7 @@ private:
// Location & Navigation
int32_t wp_bearing;
// The location of home in relation to the copter in centi-degrees
// The location of home in relation to the Sub in centi-degrees
int32_t home_bearing;
// distance between plane and home in cm
int32_t home_distance;
@ -356,8 +356,8 @@ private:
bool circle_pilot_yaw_override; // true if pilot is overriding yaw
// SIMPLE Mode
// Used to track the orientation of the copter for Simple mode. This value is reset at each arming
// or in SuperSimple mode when the copter leaves a 20m radius from home.
// Used to track the orientation of the Sub for Simple mode. This value is reset at each arming
// or in SuperSimple mode when the Sub leaves a 20m radius from home.
float simple_cos_yaw;
float simple_sin_yaw;
int32_t super_simple_last_bearing;
@ -376,7 +376,7 @@ private:
uint32_t loiter_time; // How long have we been loitering - The start time in millis
// Flip
Vector3f flip_orig_attitude; // original copter attitude before flip
Vector3f flip_orig_attitude; // original Sub attitude before flip
// Battery Sensors
AP_BattMonitor battery;
@ -398,7 +398,7 @@ private:
LowPassFilterVector3f land_accel_ef_filter; // accelerations for land and crash detector tests
// 3D Location vectors
// Current location of the copter (altitude is relative to home)
// Current location of the Sub (altitude is relative to home)
struct Location current_loc;
// Navigation Yaw control
@ -1050,12 +1050,12 @@ public:
int8_t reboot_board(uint8_t argc, const Menu::arg *argv);
};
#define MENU_FUNC(func) FUNCTOR_BIND(&copter, &Copter::func, int8_t, uint8_t, const Menu::arg *)
#define MENU_FUNC(func) FUNCTOR_BIND(&sub, &Sub::func, int8_t, uint8_t, const Menu::arg *)
extern const AP_HAL::HAL& hal;
extern Copter copter;
extern Sub sub;
using AP_HAL::millis;
using AP_HAL::micros;
#endif // _COPTER_H_
#endif // _SUB_H_

2
ArduSub/UserCode.cpp
View File

@ -1,6 +1,6 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#include "Copter.h"
#include "Sub.h"
#ifdef USERHOOK_INIT
void Copter::userhook_init()

2
ArduSub/adsb.cpp
View File

@ -17,7 +17,7 @@
* with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#include "Copter.h"
#include "Sub.h"
#if ADSB_ENABLED == ENABLED

16
ArduSub/arming_checks.cpp
View File

@ -1,9 +1,9 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#include "Copter.h"
#include "Sub.h"
// performs pre-arm checks. expects to be called at 1hz.
void Copter::update_arming_checks(void)
void Sub::update_arming_checks(void)
{
// perform pre-arm checks & display failures every 30 seconds
static uint8_t pre_arm_display_counter = PREARM_DISPLAY_PERIOD/2;
@ -20,7 +20,7 @@ void Copter::update_arming_checks(void)
}
// performs pre-arm checks and arming checks
bool Copter::all_arming_checks_passing(bool arming_from_gcs)
bool Sub::all_arming_checks_passing(bool arming_from_gcs)
{
if (pre_arm_checks(true)) {
set_pre_arm_check(true);
@ -31,7 +31,7 @@ bool Copter::all_arming_checks_passing(bool arming_from_gcs)
// perform pre-arm checks and set ap.pre_arm_check flag
// return true if the checks pass successfully
bool Copter::pre_arm_checks(bool display_failure)
bool Sub::pre_arm_checks(bool display_failure)
{
// exit immediately if already armed
if (motors.armed()) {
@ -345,7 +345,7 @@ bool Copter::pre_arm_checks(bool display_failure)
}
// perform pre_arm_rc_checks checks and set ap.pre_arm_rc_check flag
void Copter::pre_arm_rc_checks()
void Sub::pre_arm_rc_checks()
{
// exit immediately if we've already successfully performed the pre-arm rc check
if (ap.pre_arm_rc_check) {
@ -388,7 +388,7 @@ void Copter::pre_arm_rc_checks()
}
// performs pre_arm gps related checks and returns true if passed
bool Copter::pre_arm_gps_checks(bool display_failure)
bool Sub::pre_arm_gps_checks(bool display_failure)
{
// always check if inertial nav has started and is ready
if (!ahrs.healthy()) {
@ -470,7 +470,7 @@ bool Copter::pre_arm_gps_checks(bool display_failure)
}
// check ekf attitude is acceptable
bool Copter::pre_arm_ekf_attitude_check()
bool Sub::pre_arm_ekf_attitude_check()
{
// get ekf filter status
nav_filter_status filt_status = inertial_nav.get_filter_status();
@ -481,7 +481,7 @@ bool Copter::pre_arm_ekf_attitude_check()
// arm_checks - perform final checks before arming
// always called just before arming. Return true if ok to arm
// has side-effect that logging is started
bool Copter::arm_checks(bool display_failure, bool arming_from_gcs)
bool Sub::arm_checks(bool display_failure, bool arming_from_gcs)
{
#if LOGGING_ENABLED == ENABLED
// start dataflash

2
ArduSub/baro_ground_effect.cpp
View File

@ -1,6 +1,6 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#include "Copter.h"
#include "Sub.h"
#if GNDEFFECT_COMPENSATION == ENABLED
void Copter::update_ground_effect_detector(void)
{

4
ArduSub/capabilities.cpp
View File

@ -1,8 +1,8 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#include "Copter.h"
#include "Sub.h"
void Copter::init_capabilities(void)
void Sub::init_capabilities(void)
{
hal.util->set_capabilities(MAV_PROTOCOL_CAPABILITY_MISSION_FLOAT);
hal.util->set_capabilities(MAV_PROTOCOL_CAPABILITY_PARAM_FLOAT);

18
ArduSub/commands.cpp
View File

@ -1,6 +1,6 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#include "Copter.h"
#include "Sub.h"
/*
* the home_state has a number of possible values (see enum HomeState in defines.h's)
@ -10,7 +10,7 @@
*/
// checks if we should update ahrs/RTL home position from the EKF
void Copter::update_home_from_EKF()
void Sub::update_home_from_EKF()
{
// exit immediately if home already set
if (ap.home_state != HOME_UNSET) {
@ -27,7 +27,7 @@ void Copter::update_home_from_EKF()
}
// set_home_to_current_location_inflight - set home to current GPS location (horizontally) and EKF origin vertically
void Copter::set_home_to_current_location_inflight() {
void Sub::set_home_to_current_location_inflight() {
// get current location from EKF
Location temp_loc;
if (inertial_nav.get_location(temp_loc)) {
@ -38,7 +38,7 @@ void Copter::set_home_to_current_location_inflight() {
}
// set_home_to_current_location - set home to current GPS location
bool Copter::set_home_to_current_location() {
bool Sub::set_home_to_current_location() {
// get current location from EKF
Location temp_loc;
if (inertial_nav.get_location(temp_loc)) {
@ -48,7 +48,7 @@ bool Copter::set_home_to_current_location() {
}
// set_home_to_current_location_and_lock - set home to current location and lock so it cannot be moved
bool Copter::set_home_to_current_location_and_lock()
bool Sub::set_home_to_current_location_and_lock()
{
if (set_home_to_current_location()) {
set_home_state(HOME_SET_AND_LOCKED);
@ -59,7 +59,7 @@ bool Copter::set_home_to_current_location_and_lock()
// set_home_and_lock - sets ahrs home (used for RTL) to specified location and locks so it cannot be moved
// unless this function is called again
bool Copter::set_home_and_lock(const Location& loc)
bool Sub::set_home_and_lock(const Location& loc)
{
if (set_home(loc)) {
set_home_state(HOME_SET_AND_LOCKED);
@ -71,7 +71,7 @@ bool Copter::set_home_and_lock(const Location& loc)
// set_home - sets ahrs home (used for RTL) to specified location
// initialises inertial nav and compass on first call
// returns true if home location set successfully
bool Copter::set_home(const Location& loc)
bool Sub::set_home(const Location& loc)
{
// check location is valid
if (loc.lat == 0 && loc.lng == 0) {
@ -113,7 +113,7 @@ bool Copter::set_home(const Location& loc)
// far_from_EKF_origin - checks if a location is too far from the EKF origin
// returns true if too far
bool Copter::far_from_EKF_origin(const Location& loc)
bool Sub::far_from_EKF_origin(const Location& loc)
{
// check distance to EKF origin
const struct Location &ekf_origin = inertial_nav.get_origin();
@ -126,7 +126,7 @@ bool Copter::far_from_EKF_origin(const Location& loc)
}
// checks if we should update ahrs/RTL home position from GPS
void Copter::set_system_time_from_GPS()
void Sub::set_system_time_from_GPS()
{
// exit immediately if system time already set
if (ap.system_time_set) {

86
ArduSub/commands_logic.cpp
View File

@ -1,9 +1,9 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#include "Copter.h"
#include "Sub.h"
// start_command - this function will be called when the ap_mission lib wishes to start a new command
bool Copter::start_command(const AP_Mission::Mission_Command& cmd)
bool Sub::start_command(const AP_Mission::Mission_Command& cmd)
{
// To-Do: logging when new commands start/end
if (should_log(MASK_LOG_CMD)) {
@ -161,7 +161,7 @@ bool Copter::start_command(const AP_Mission::Mission_Command& cmd)
// verify_command_callback - callback function called from ap-mission at 10hz or higher when a command is being run
// we double check that the flight mode is AUTO to avoid the possibility of ap-mission triggering actions while we're not in AUTO mode
bool Copter::verify_command_callback(const AP_Mission::Mission_Command& cmd)
bool Sub::verify_command_callback(const AP_Mission::Mission_Command& cmd)
{
if (control_mode == AUTO) {
bool cmd_complete = verify_command(cmd);
@ -180,7 +180,7 @@ bool Copter::verify_command_callback(const AP_Mission::Mission_Command& cmd)
// verify_command - this will be called repeatedly by ap_mission lib to ensure the active commands are progressing
// should return true once the active navigation command completes successfully
// called at 10hz or higher
bool Copter::verify_command(const AP_Mission::Mission_Command& cmd)
bool Sub::verify_command(const AP_Mission::Mission_Command& cmd)
{
switch(cmd.id) {
@ -242,7 +242,7 @@ bool Copter::verify_command(const AP_Mission::Mission_Command& cmd)
}
// exit_mission - function that is called once the mission completes
void Copter::exit_mission()
void Sub::exit_mission()
{
// play a tone
AP_Notify::events.mission_complete = 1;
@ -270,7 +270,7 @@ void Copter::exit_mission()
/********************************************************************************/
// do_RTL - start Return-to-Launch
void Copter::do_RTL(void)
void Sub::do_RTL(void)
{
// start rtl in auto flight mode
auto_rtl_start();
@ -281,7 +281,7 @@ void Copter::do_RTL(void)
/********************************************************************************/
// do_takeoff - initiate takeoff navigation command
void Copter::do_takeoff(const AP_Mission::Mission_Command& cmd)
void Sub::do_takeoff(const AP_Mission::Mission_Command& cmd)
{
// Set wp navigation target to safe altitude above current position
float takeoff_alt = cmd.content.location.alt;
@ -291,7 +291,7 @@ void Copter::do_takeoff(const AP_Mission::Mission_Command& cmd)
}
// do_nav_wp - initiate move to next waypoint
void Copter::do_nav_wp(const AP_Mission::Mission_Command& cmd)
void Sub::do_nav_wp(const AP_Mission::Mission_Command& cmd)
{
const Vector3f &curr_pos = inertial_nav.get_position();
const Vector3f local_pos = pv_location_to_vector_with_default(cmd.content.location, curr_pos);
@ -310,7 +310,7 @@ void Copter::do_nav_wp(const AP_Mission::Mission_Command& cmd)
}
// do_land - initiate landing procedure
void Copter::do_land(const AP_Mission::Mission_Command& cmd)
void Sub::do_land(const AP_Mission::Mission_Command& cmd)
{
// To-Do: check if we have already landed
@ -334,7 +334,7 @@ void Copter::do_land(const AP_Mission::Mission_Command& cmd)
// do_loiter_unlimited - start loitering with no end conditions
// note: caller should set yaw_mode
void Copter::do_loiter_unlimited(const AP_Mission::Mission_Command& cmd)
void Sub::do_loiter_unlimited(const AP_Mission::Mission_Command& cmd)
{
Vector3f target_pos;
@ -360,7 +360,7 @@ void Copter::do_loiter_unlimited(const AP_Mission::Mission_Command& cmd)
}
// do_circle - initiate moving in a circle
void Copter::do_circle(const AP_Mission::Mission_Command& cmd)
void Sub::do_circle(const AP_Mission::Mission_Command& cmd)
{
Vector3f curr_pos = inertial_nav.get_position();
Vector3f circle_center = pv_location_to_vector(cmd.content.location);
@ -403,7 +403,7 @@ void Copter::do_circle(const AP_Mission::Mission_Command& cmd)
// do_loiter_time - initiate loitering at a point for a given time period
// note: caller should set yaw_mode
void Copter::do_loiter_time(const AP_Mission::Mission_Command& cmd)
void Sub::do_loiter_time(const AP_Mission::Mission_Command& cmd)
{
Vector3f target_pos;
@ -432,7 +432,7 @@ void Copter::do_loiter_time(const AP_Mission::Mission_Command& cmd)
}
// do_spline_wp - initiate move to next waypoint
void Copter::do_spline_wp(const AP_Mission::Mission_Command& cmd)
void Sub::do_spline_wp(const AP_Mission::Mission_Command& cmd)
{
const Vector3f& curr_pos = inertial_nav.get_position();
Vector3f local_pos = pv_location_to_vector_with_default(cmd.content.location, curr_pos);
@ -477,7 +477,7 @@ void Copter::do_spline_wp(const AP_Mission::Mission_Command& cmd)
#if NAV_GUIDED == ENABLED
// do_nav_guided_enable - initiate accepting commands from external nav computer
void Copter::do_nav_guided_enable(const AP_Mission::Mission_Command& cmd)
void Sub::do_nav_guided_enable(const AP_Mission::Mission_Command& cmd)
{
if (cmd.p1 > 0) {
// initialise guided limits
@ -492,7 +492,7 @@ void Copter::do_nav_guided_enable(const AP_Mission::Mission_Command& cmd)
#if PARACHUTE == ENABLED
// do_parachute - configure or release parachute
void Copter::do_parachute(const AP_Mission::Mission_Command& cmd)
void Sub::do_parachute(const AP_Mission::Mission_Command& cmd)
{
switch (cmd.p1) {
case PARACHUTE_DISABLE:
@ -515,7 +515,7 @@ void Copter::do_parachute(const AP_Mission::Mission_Command& cmd)
#if EPM_ENABLED == ENABLED
// do_gripper - control EPM gripper
void Copter::do_gripper(const AP_Mission::Mission_Command& cmd)
void Sub::do_gripper(const AP_Mission::Mission_Command& cmd)
{
// Note: we ignore the gripper num parameter because we only support one gripper
switch (cmd.content.gripper.action) {
@ -536,7 +536,7 @@ void Copter::do_gripper(const AP_Mission::Mission_Command& cmd)
#if NAV_GUIDED == ENABLED
// do_guided_limits - pass guided limits to guided controller
void Copter::do_guided_limits(const AP_Mission::Mission_Command& cmd)
void Sub::do_guided_limits(const AP_Mission::Mission_Command& cmd)
{
guided_limit_set(cmd.p1 * 1000, // convert seconds to ms
cmd.content.guided_limits.alt_min * 100.0f, // convert meters to cm
@ -550,14 +550,14 @@ void Copter::do_guided_limits(const AP_Mission::Mission_Command& cmd)
/********************************************************************************/
// verify_takeoff - check if we have completed the takeoff
bool Copter::verify_takeoff()
bool Sub::verify_takeoff()
{
// have we reached our target altitude?
return wp_nav.reached_wp_destination();
}
// verify_land - returns true if landing has been completed
bool Copter::verify_land()
bool Sub::verify_land()
{
bool retval = false;
@ -593,7 +593,7 @@ bool Copter::verify_land()
}
// verify_nav_wp - check if we have reached the next way point
bool Copter::verify_nav_wp(const AP_Mission::Mission_Command& cmd)
bool Sub::verify_nav_wp(const AP_Mission::Mission_Command& cmd)
{
// check if we have reached the waypoint
if( !wp_nav.reached_wp_destination() ) {
@ -617,13 +617,13 @@ bool Copter::verify_nav_wp(const AP_Mission::Mission_Command& cmd)
}
}
bool Copter::verify_loiter_unlimited()
bool Sub::verify_loiter_unlimited()
{
return false;
}
// verify_loiter_time - check if we have loitered long enough
bool Copter::verify_loiter_time()
bool Sub::verify_loiter_time()
{
// return immediately if we haven't reached our destination
if (!wp_nav.reached_wp_destination()) {
@ -640,7 +640,7 @@ bool Copter::verify_loiter_time()
}
// verify_circle - check if we have circled the point enough
bool Copter::verify_circle(const AP_Mission::Mission_Command& cmd)
bool Sub::verify_circle(const AP_Mission::Mission_Command& cmd)
{
// check if we've reached the edge
if (auto_mode == Auto_CircleMoveToEdge) {
@ -672,13 +672,13 @@ bool Copter::verify_circle(const AP_Mission::Mission_Command& cmd)
// verify_RTL - handles any state changes required to implement RTL
// do_RTL should have been called once first to initialise all variables
// returns true with RTL has completed successfully
bool Copter::verify_RTL()
bool Sub::verify_RTL()
{
return (rtl_state_complete && (rtl_state == RTL_FinalDescent || rtl_state == RTL_Land));
}
// verify_spline_wp - check if we have reached the next way point using spline
bool Copter::verify_spline_wp(const AP_Mission::Mission_Command& cmd)
bool Sub::verify_spline_wp(const AP_Mission::Mission_Command& cmd)
{
// check if we have reached the waypoint
if( !wp_nav.reached_wp_destination() ) {
@ -701,7 +701,7 @@ bool Copter::verify_spline_wp(const AP_Mission::Mission_Command& cmd)
#if NAV_GUIDED == ENABLED
// verify_nav_guided - check if we have breached any limits
bool Copter::verify_nav_guided_enable(const AP_Mission::Mission_Command& cmd)
bool Sub::verify_nav_guided_enable(const AP_Mission::Mission_Command& cmd)
{
// if disabling guided mode then immediately return true so we move to next command
if (cmd.p1 == 0) {
@ -718,23 +718,23 @@ bool Copter::verify_nav_guided_enable(const AP_Mission::Mission_Command& cmd)
// Condition (May) commands
/********************************************************************************/
void Copter::do_wait_delay(const AP_Mission::Mission_Command& cmd)
void Sub::do_wait_delay(const AP_Mission::Mission_Command& cmd)
{
condition_start = millis();
condition_value = cmd.content.delay.seconds * 1000; // convert seconds to milliseconds
}
void Copter::do_change_alt(const AP_Mission::Mission_Command& cmd)
void Sub::do_change_alt(const AP_Mission::Mission_Command& cmd)
{
// To-Do: store desired altitude in a variable so that it can be verified later
}
void Copter::do_within_distance(const AP_Mission::Mission_Command& cmd)
void Sub::do_within_distance(const AP_Mission::Mission_Command& cmd)
{
condition_value = cmd.content.distance.meters * 100;
}
void Copter::do_yaw(const AP_Mission::Mission_Command& cmd)
void Sub::do_yaw(const AP_Mission::Mission_Command& cmd)
{
set_auto_yaw_look_at_heading(
cmd.content.yaw.angle_deg,
@ -748,7 +748,7 @@ void Copter::do_yaw(const AP_Mission::Mission_Command& cmd)
// Verify Condition (May) commands
/********************************************************************************/
bool Copter::verify_wait_delay()
bool Sub::verify_wait_delay()
{
if (millis() - condition_start > (uint32_t)MAX(condition_value,0)) {
condition_value = 0;
@ -757,13 +757,13 @@ bool Copter::verify_wait_delay()
return false;
}
bool Copter::verify_change_alt()
bool Sub::verify_change_alt()
{
// To-Do: use recorded target altitude to verify we have reached the target
return true;
}
bool Copter::verify_within_distance()
bool Sub::verify_within_distance()
{
// update distance calculation
calc_wp_distance();
@ -775,7 +775,7 @@ bool Copter::verify_within_distance()
}
// verify_yaw - return true if we have reached the desired heading
bool Copter::verify_yaw()
bool Sub::verify_yaw()
{
// set yaw mode if it has been changed (the waypoint controller often retakes control of yaw as it executes a new waypoint command)
if (auto_yaw_mode != AUTO_YAW_LOOK_AT_HEADING) {
@ -795,7 +795,7 @@ bool Copter::verify_yaw()
/********************************************************************************/
// do_guided - start guided mode
bool Copter::do_guided(const AP_Mission::Mission_Command& cmd)
bool Sub::do_guided(const AP_Mission::Mission_Command& cmd)
{
Vector3f pos_or_vel; // target location or velocity
@ -828,14 +828,14 @@ bool Copter::do_guided(const AP_Mission::Mission_Command& cmd)
return true;
}
void Copter::do_change_speed(const AP_Mission::Mission_Command& cmd)
void Sub::do_change_speed(const AP_Mission::Mission_Command& cmd)
{
if (cmd.content.speed.target_ms > 0) {
wp_nav.set_speed_xy(cmd.content.speed.target_ms * 100.0f);
}
}
void Copter::do_set_home(const AP_Mission::Mission_Command& cmd)
void Sub::do_set_home(const AP_Mission::Mission_Command& cmd)
{
if(cmd.p1 == 1 || (cmd.content.location.lat == 0 && cmd.content.location.lng == 0 && cmd.content.location.alt == 0)) {
set_home_to_current_location();
@ -850,13 +850,13 @@ void Copter::do_set_home(const AP_Mission::Mission_Command& cmd)
// this involves either moving the camera to point at the ROI (region of interest)
// and possibly rotating the copter to point at the ROI if our mount type does not support a yaw feature
// TO-DO: add support for other features of MAV_CMD_DO_SET_ROI including pointing at a given waypoint
void Copter::do_roi(const AP_Mission::Mission_Command& cmd)
void Sub::do_roi(const AP_Mission::Mission_Command& cmd)
{
set_auto_yaw_roi(cmd.content.location);
}
// do_digicam_configure Send Digicam Configure message with the camera library
void Copter::do_digicam_configure(const AP_Mission::Mission_Command& cmd)
void Sub::do_digicam_configure(const AP_Mission::Mission_Command& cmd)
{
#if CAMERA == ENABLED
camera.configure(cmd.content.digicam_configure.shooting_mode,
@ -870,7 +870,7 @@ void Copter::do_digicam_configure(const AP_Mission::Mission_Command& cmd)
}
// do_digicam_control Send Digicam Control message with the camera library
void Copter::do_digicam_control(const AP_Mission::Mission_Command& cmd)
void Sub::do_digicam_control(const AP_Mission::Mission_Command& cmd)
{
#if CAMERA == ENABLED
camera.control(cmd.content.digicam_control.session,
@ -884,7 +884,7 @@ void Copter::do_digicam_control(const AP_Mission::Mission_Command& cmd)
}
// do_take_picture - take a picture with the camera library
void Copter::do_take_picture()
void Sub::do_take_picture()
{
#if CAMERA == ENABLED
camera.trigger_pic(true);
@ -893,7 +893,7 @@ void Copter::do_take_picture()
}
// log_picture - log picture taken and send feedback to GCS
void Copter::log_picture()
void Sub::log_picture()
{
gcs_send_message(MSG_CAMERA_FEEDBACK);
if (should_log(MASK_LOG_CAMERA)) {
@ -902,7 +902,7 @@ void Copter::log_picture()
}
// point the camera to a specified angle
void Copter::do_mount_control(const AP_Mission::Mission_Command& cmd)
void Sub::do_mount_control(const AP_Mission::Mission_Command& cmd)
{
#if MOUNT == ENABLED
camera_mount.set_angle_targets(cmd.content.mount_control.roll, cmd.content.mount_control.pitch, cmd.content.mount_control.yaw);

4
ArduSub/compassmot.cpp
View File

@ -1,13 +1,13 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#include "Copter.h"
#include "Sub.h"
/*
compass/motor interference calibration
*/
// setup_compassmot - sets compass's motor interference parameters
uint8_t Copter::mavlink_compassmot(mavlink_channel_t chan)
uint8_t Sub::mavlink_compassmot(mavlink_channel_t chan)
{
#if FRAME_CONFIG == HELI_FRAME
// compassmot not implemented for tradheli

4
ArduSub/compat.cpp
View File

@ -1,6 +1,6 @@
#include "Copter.h"
#include "Sub.h"
void Copter::delay(uint32_t ms)
void Sub::delay(uint32_t ms)
{
hal.scheduler->delay(ms);
}

8
ArduSub/control_acro.cpp
View File

@ -1,5 +1,5 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#include "Copter.h"
#include "Sub.h"
/*
@ -7,7 +7,7 @@
*/
// acro_init - initialise acro controller
bool Copter::acro_init(bool ignore_checks)
bool Sub::acro_init(bool ignore_checks)
{
// if landed and the mode we're switching from does not have manual throttle and the throttle stick is too high
if (motors.armed() && ap.land_complete && !mode_has_manual_throttle(control_mode) && (g.rc_3.control_in > get_non_takeoff_throttle())) {
@ -21,7 +21,7 @@ bool Copter::acro_init(bool ignore_checks)
// acro_run - runs the acro controller
// should be called at 100hz or more
void Copter::acro_run()
void Sub::acro_run()
{
float target_roll, target_pitch, target_yaw;
int16_t pilot_throttle_scaled;
@ -57,7 +57,7 @@ void Copter::acro_run()
// get_pilot_desired_angle_rates - transform pilot's roll pitch and yaw input into a desired lean angle rates
// returns desired angle rates in centi-degrees-per-second
void Copter::get_pilot_desired_angle_rates(int16_t roll_in, int16_t pitch_in, int16_t yaw_in, float &roll_out, float &pitch_out, float &yaw_out)
void Sub::get_pilot_desired_angle_rates(int16_t roll_in, int16_t pitch_in, int16_t yaw_in, float &roll_out, float &pitch_out, float &yaw_out)
{
float rate_limit;
Vector3f rate_ef_level, rate_bf_level, rate_bf_request;

6
ArduSub/control_althold.cpp
View File

@ -1,5 +1,5 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#include "Copter.h"
#include "Sub.h"
/*
@ -7,7 +7,7 @@
*/
// althold_init - initialise althold controller
bool Copter::althold_init(bool ignore_checks)
bool Sub::althold_init(bool ignore_checks)
{
#if FRAME_CONFIG == HELI_FRAME
// do not allow helis to enter Alt Hold if the Rotor Runup is not complete
@ -32,7 +32,7 @@ bool Copter::althold_init(bool ignore_checks)
// althold_run - runs the althold controller
// should be called at 100hz or more
void Copter::althold_run()
void Sub::althold_run()
{
AltHoldModeState althold_state;
float takeoff_climb_rate = 0.0f;

52
ArduSub/control_auto.cpp
View File

@ -1,6 +1,6 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#include "Copter.h"
#include "Sub.h"
/*
* control_auto.pde - init and run calls for auto flight mode
@ -20,7 +20,7 @@
*/
// auto_init - initialise auto controller
bool Copter::auto_init(bool ignore_checks)
bool Sub::auto_init(bool ignore_checks)
{
if ((position_ok() && mission.num_commands() > 1) || ignore_checks) {
auto_mode = Auto_Loiter;
@ -48,7 +48,7 @@ bool Copter::auto_init(bool ignore_checks)
// auto_run - runs the auto controller
// should be called at 100hz or more
// relies on run_autopilot being called at 10hz which handles decision making and non-navigation related commands
void Copter::auto_run()
void Sub::auto_run()
{
// call the correct auto controller
switch (auto_mode) {
@ -91,7 +91,7 @@ void Copter::auto_run()
}
// auto_takeoff_start - initialises waypoint controller to implement take-off
void Copter::auto_takeoff_start(float final_alt_above_home)
void Sub::auto_takeoff_start(float final_alt_above_home)
{
auto_mode = Auto_TakeOff;
@ -109,7 +109,7 @@ void Copter::auto_takeoff_start(float final_alt_above_home)
// auto_takeoff_run - takeoff in auto mode
// called by auto_run at 100hz or more
void Copter::auto_takeoff_run()
void Sub::auto_takeoff_run()
{
// if not auto armed or motor interlock not enabled set throttle to zero and exit immediately
if(!ap.auto_armed || !motors.get_interlock()) {
@ -146,7 +146,7 @@ void Copter::auto_takeoff_run()
}
// auto_wp_start - initialises waypoint controller to implement flying to a particular destination
void Copter::auto_wp_start(const Vector3f& destination)
void Sub::auto_wp_start(const Vector3f& destination)
{
auto_mode = Auto_WP;
@ -162,7 +162,7 @@ void Copter::auto_wp_start(const Vector3f& destination)
// auto_wp_run - runs the auto waypoint controller
// called by auto_run at 100hz or more
void Copter::auto_wp_run()
void Sub::auto_wp_run()
{
// if not auto armed or motor interlock not enabled set throttle to zero and exit immediately
if(!ap.auto_armed || !motors.get_interlock()) {
@ -208,7 +208,7 @@ void Copter::auto_wp_run()
// auto_spline_start - initialises waypoint controller to implement flying to a particular destination using the spline controller
// seg_end_type can be SEGMENT_END_STOP, SEGMENT_END_STRAIGHT or SEGMENT_END_SPLINE. If Straight or Spline the next_destination should be provided
void Copter::auto_spline_start(const Vector3f& destination, bool stopped_at_start,
void Sub::auto_spline_start(const Vector3f& destination, bool stopped_at_start,
AC_WPNav::spline_segment_end_type seg_end_type,
const Vector3f& next_destination)
{
@ -226,7 +226,7 @@ void Copter::auto_spline_start(const Vector3f& destination, bool stopped_at_star
// auto_spline_run - runs the auto spline controller
// called by auto_run at 100hz or more
void Copter::auto_spline_run()
void Sub::auto_spline_run()
{
// if not auto armed or motor interlock not enabled set throttle to zero and exit immediately
if(!ap.auto_armed || !motors.get_interlock()) {
@ -271,7 +271,7 @@ void Copter::auto_spline_run()
}
// auto_land_start - initialises controller to implement a landing
void Copter::auto_land_start()
void Sub::auto_land_start()
{
// set target to stopping point
Vector3f stopping_point;
@ -282,7 +282,7 @@ void Copter::auto_land_start()
}
// auto_land_start - initialises controller to implement a landing
void Copter::auto_land_start(const Vector3f& destination)
void Sub::auto_land_start(const Vector3f& destination)
{
auto_mode = Auto_Land;
@ -298,7 +298,7 @@ void Copter::auto_land_start(const Vector3f& destination)
// auto_land_run - lands in auto mode
// called by auto_run at 100hz or more
void Copter::auto_land_run()
void Sub::auto_land_run()
{
int16_t roll_control = 0, pitch_control = 0;
float target_yaw_rate = 0;
@ -356,7 +356,7 @@ void Copter::auto_land_run()
}
// auto_rtl_start - initialises RTL in AUTO flight mode
void Copter::auto_rtl_start()
void Sub::auto_rtl_start()
{
auto_mode = Auto_RTL;
@ -366,7 +366,7 @@ void Copter::auto_rtl_start()
// auto_rtl_run - rtl in AUTO flight mode
// called by auto_run at 100hz or more
void Copter::auto_rtl_run()
void Sub::auto_rtl_run()
{
// call regular rtl flight mode run function
rtl_run();
@ -375,7 +375,7 @@ void Copter::auto_rtl_run()
// auto_circle_movetoedge_start - initialise waypoint controller to move to edge of a circle with it's center at the specified location
// we assume the caller has set the circle's circle with circle_nav.set_center()
// we assume the caller has performed all required GPS_ok checks
void Copter::auto_circle_movetoedge_start()
void Sub::auto_circle_movetoedge_start()
{
// check our distance from edge of circle
Vector3f circle_edge;
@ -400,7 +400,7 @@ void Copter::auto_circle_movetoedge_start()
}
// auto_circle_start - initialises controller to fly a circle in AUTO flight mode
void Copter::auto_circle_start()
void Sub::auto_circle_start()
{
auto_mode = Auto_Circle;
@ -411,7 +411,7 @@ void Copter::auto_circle_start()
// auto_circle_run - circle in AUTO flight mode
// called by auto_run at 100hz or more
void Copter::auto_circle_run()
void Sub::auto_circle_run()
{
// call circle controller
circle_nav.update();
@ -425,7 +425,7 @@ void Copter::auto_circle_run()
#if NAV_GUIDED == ENABLED
// auto_nav_guided_start - hand over control to external navigation controller in AUTO mode
void Copter::auto_nav_guided_start()
void Sub::auto_nav_guided_start()
{
auto_mode = Auto_NavGuided;
@ -438,7 +438,7 @@ void Copter::auto_nav_guided_start()
// auto_nav_guided_run - allows control by external navigation controller
// called by auto_run at 100hz or more
void Copter::auto_nav_guided_run()
void Sub::auto_nav_guided_run()
{
// call regular guided flight mode run function
guided_run();
@ -447,7 +447,7 @@ void Copter::auto_nav_guided_run()
// auto_loiter_start - initialises loitering in auto mode
// returns success/failure because this can be called by exit_mission
bool Copter::auto_loiter_start()
bool Sub::auto_loiter_start()
{
// return failure if GPS is bad
if (!position_ok()) {
@ -473,7 +473,7 @@ bool Copter::auto_loiter_start()
// auto_loiter_run - loiter in AUTO flight mode
// called by auto_run at 100hz or more
void Copter::auto_loiter_run()
void Sub::auto_loiter_run()
{
// if not auto armed or motor interlock not enabled set throttle to zero and exit immediately
if(!ap.auto_armed || ap.land_complete || !motors.get_interlock()) {
@ -501,7 +501,7 @@ void Copter::auto_loiter_run()
// get_default_auto_yaw_mode - returns auto_yaw_mode based on WP_YAW_BEHAVIOR parameter
// set rtl parameter to true if this is during an RTL
uint8_t Copter::get_default_auto_yaw_mode(bool rtl)
uint8_t Sub::get_default_auto_yaw_mode(bool rtl)
{
switch (g.wp_yaw_behavior) {
@ -529,7 +529,7 @@ uint8_t Copter::get_default_auto_yaw_mode(bool rtl)
}
// set_auto_yaw_mode - sets the yaw mode for auto
void Copter::set_auto_yaw_mode(uint8_t yaw_mode)
void Sub::set_auto_yaw_mode(uint8_t yaw_mode)
{
// return immediately if no change
if (auto_yaw_mode == yaw_mode) {
@ -566,7 +566,7 @@ void Copter::set_auto_yaw_mode(uint8_t yaw_mode)
}
// set_auto_yaw_look_at_heading - sets the yaw look at heading for auto mode
void Copter::set_auto_yaw_look_at_heading(float angle_deg, float turn_rate_dps, int8_t direction, uint8_t relative_angle)
void Sub::set_auto_yaw_look_at_heading(float angle_deg, float turn_rate_dps, int8_t direction, uint8_t relative_angle)
{
// get current yaw target
int32_t curr_yaw_target = attitude_control.get_att_target_euler_cd().z;
@ -599,7 +599,7 @@ void Copter::set_auto_yaw_look_at_heading(float angle_deg, float turn_rate_dps,
}
// set_auto_yaw_roi - sets the yaw to look at roi for auto mode
void Copter::set_auto_yaw_roi(const Location &roi_location)
void Sub::set_auto_yaw_roi(const Location &roi_location)
{
// if location is zero lat, lon and altitude turn off ROI
if (roi_location.alt == 0 && roi_location.lat == 0 && roi_location.lng == 0) {
@ -637,7 +637,7 @@ void Copter::set_auto_yaw_roi(const Location &roi_location)
// get_auto_heading - returns target heading depending upon auto_yaw_mode
// 100hz update rate
float Copter::get_auto_heading(void)
float Sub::get_auto_heading(void)
{
switch(auto_yaw_mode) {

2
ArduSub/control_autotune.cpp
View File

@ -1,6 +1,6 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#include "Copter.h"
#include "Sub.h"
#if AUTOTUNE_ENABLED == ENABLED

6
ArduSub/control_brake.cpp
View File

@ -1,13 +1,13 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#include "Copter.h"
#include "Sub.h"
/*
* control_brake.pde - init and run calls for brake flight mode
*/
// brake_init - initialise brake controller
bool Copter::brake_init(bool ignore_checks)
bool Sub::brake_init(bool ignore_checks)
{
if (position_ok() || ignore_checks) {
@ -33,7 +33,7 @@ bool Copter::brake_init(bool ignore_checks)
// brake_run - runs the brake controller
// should be called at 100hz or more
void Copter::brake_run()
void Sub::brake_run()
{
// if not auto armed set throttle to zero and exit immediately
if(!ap.auto_armed) {

6
ArduSub/control_circle.cpp
View File

@ -1,13 +1,13 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#include "Copter.h"
#include "Sub.h"
/*
* control_circle.pde - init and run calls for circle flight mode
*/
// circle_init - initialise circle controller flight mode
bool Copter::circle_init(bool ignore_checks)
bool Sub::circle_init(bool ignore_checks)
{
if (position_ok() || ignore_checks) {
circle_pilot_yaw_override = false;
@ -30,7 +30,7 @@ bool Copter::circle_init(bool ignore_checks)
// circle_run - runs the circle flight mode
// should be called at 100hz or more
void Copter::circle_run()
void Sub::circle_run()
{
float target_yaw_rate = 0;
float target_climb_rate = 0;

8
ArduSub/control_drift.cpp
View File

@ -1,6 +1,6 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#include "Copter.h"
#include "Sub.h"
/*
* control_drift.pde - init and run calls for drift flight mode
@ -29,7 +29,7 @@
#endif
// drift_init - initialise drift controller
bool Copter::drift_init(bool ignore_checks)
bool Sub::drift_init(bool ignore_checks)
{
if (position_ok() || ignore_checks) {
return true;
@ -40,7 +40,7 @@ bool Copter::drift_init(bool ignore_checks)
// drift_run - runs the drift controller
// should be called at 100hz or more
void Copter::drift_run()
void Sub::drift_run()
{
static float breaker = 0.0f;
static float roll_input = 0.0f;
@ -101,7 +101,7 @@ void Copter::drift_run()
}
// get_throttle_assist - return throttle output (range 0 ~ 1000) based on pilot input and z-axis velocity
int16_t Copter::get_throttle_assist(float velz, int16_t pilot_throttle_scaled)
int16_t Sub::get_throttle_assist(float velz, int16_t pilot_throttle_scaled)
{
// throttle assist - adjusts throttle to slow the vehicle's vertical velocity
// Only active when pilot's throttle is between 213 ~ 787

6
ArduSub/control_flip.cpp
View File

@ -1,6 +1,6 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#include "Copter.h"
#include "Sub.h"
/*
* control_flip.pde - init and run calls for flip flight mode
@ -39,7 +39,7 @@ int8_t flip_roll_dir; // roll direction (-1 = roll left, 1 = roll
int8_t flip_pitch_dir; // pitch direction (-1 = pitch forward, 1 = pitch back)
// flip_init - initialise flip controller
bool Copter::flip_init(bool ignore_checks)
bool Sub::flip_init(bool ignore_checks)
{
// only allow flip from ACRO, Stabilize, AltHold or Drift flight modes
if (control_mode != ACRO && control_mode != STABILIZE && control_mode != ALT_HOLD) {
@ -94,7 +94,7 @@ bool Copter::flip_init(bool ignore_checks)
// flip_run - runs the flip controller
// should be called at 100hz or more
void Copter::flip_run()
void Sub::flip_run()
{
int16_t throttle_out;
float recovery_angle;

42
ArduSub/control_guided.cpp
View File

@ -1,6 +1,6 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#include "Copter.h"
#include "Sub.h"
/*
* control_guided.pde - init and run calls for guided flight mode
@ -36,7 +36,7 @@ struct Guided_Limit {
} guided_limit;
// guided_init - initialise guided controller
bool Copter::guided_init(bool ignore_checks)
bool Sub::guided_init(bool ignore_checks)
{
if (position_ok() || ignore_checks) {
// initialise yaw
@ -51,7 +51,7 @@ bool Copter::guided_init(bool ignore_checks)
// guided_takeoff_start - initialises waypoint controller to implement take-off
void Copter::guided_takeoff_start(float final_alt_above_home)
void Sub::guided_takeoff_start(float final_alt_above_home)
{
guided_mode = Guided_TakeOff;
@ -68,7 +68,7 @@ void Copter::guided_takeoff_start(float final_alt_above_home)
}
// initialise guided mode's position controller
void Copter::guided_pos_control_start()
void Sub::guided_pos_control_start()
{
// set to position control mode
guided_mode = Guided_WP;
@ -89,7 +89,7 @@ void Copter::guided_pos_control_start()
}
// initialise guided mode's velocity controller
void Copter::guided_vel_control_start()
void Sub::guided_vel_control_start()
{
// set guided_mode to velocity controller
guided_mode = Guided_Velocity;
@ -103,7 +103,7 @@ void Copter::guided_vel_control_start()
}
// initialise guided mode's posvel controller
void Copter::guided_posvel_control_start()
void Sub::guided_posvel_control_start()
{
// set guided_mode to velocity controller
guided_mode = Guided_PosVel;
@ -131,7 +131,7 @@ void Copter::guided_posvel_control_start()
}
// initialise guided mode's angle controller
void Copter::guided_angle_control_start()
void Sub::guided_angle_control_start()
{
// set guided_mode to velocity controller
guided_mode = Guided_Angle;
@ -156,7 +156,7 @@ void Copter::guided_angle_control_start()
}
// guided_set_destination - sets guided mode's target destination
void Copter::guided_set_destination(const Vector3f& destination)
void Sub::guided_set_destination(const Vector3f& destination)
{
// ensure we are in position control mode
if (guided_mode != Guided_WP) {
@ -167,7 +167,7 @@ void Copter::guided_set_destination(const Vector3f& destination)
}
// guided_set_velocity - sets guided mode's target velocity
void Copter::guided_set_velocity(const Vector3f& velocity)
void Sub::guided_set_velocity(const Vector3f& velocity)
{
// check we are in velocity control mode
if (guided_mode != Guided_Velocity) {
@ -181,7 +181,7 @@ void Copter::guided_set_velocity(const Vector3f& velocity)
}
// set guided mode posvel target
void Copter::guided_set_destination_posvel(const Vector3f& destination, const Vector3f& velocity) {
void Sub::guided_set_destination_posvel(const Vector3f& destination, const Vector3f& velocity) {
// check we are in velocity control mode
if (guided_mode != Guided_PosVel) {
guided_posvel_control_start();
@ -195,7 +195,7 @@ void Copter::guided_set_destination_posvel(const Vector3f& destination, const Ve
}
// set guided mode angle target
void Copter::guided_set_angle(const Quaternion &q, float climb_rate_cms)
void Sub::guided_set_angle(const Quaternion &q, float climb_rate_cms)
{
// check we are in velocity control mode
if (guided_mode != Guided_Angle) {
@ -214,7 +214,7 @@ void Copter::guided_set_angle(const Quaternion &q, float climb_rate_cms)
// guided_run - runs the guided controller
// should be called at 100hz or more
void Copter::guided_run()
void Sub::guided_run()
{
// call the correct auto controller
switch (guided_mode) {
@ -248,7 +248,7 @@ void Copter::guided_run()
// guided_takeoff_run - takeoff in guided mode
// called by guided_run at 100hz or more
void Copter::guided_takeoff_run()
void Sub::guided_takeoff_run()
{
// if not auto armed or motors not enabled set throttle to zero and exit immediately
if (!ap.auto_armed || !motors.get_interlock()) {
@ -281,7 +281,7 @@ void Copter::guided_takeoff_run()
// guided_pos_control_run - runs the guided position controller
// called from guided_run
void Copter::guided_pos_control_run()
void Sub::guided_pos_control_run()
{
// if not auto armed or motors not enabled set throttle to zero and exit immediately
if (!ap.auto_armed || !motors.get_interlock() || ap.land_complete) {
@ -323,7 +323,7 @@ void Copter::guided_pos_control_run()
// guided_vel_control_run - runs the guided velocity controller
// called from guided_run
void Copter::guided_vel_control_run()
void Sub::guided_vel_control_run()
{
// if not auto armed or motors not enabled set throttle to zero and exit immediately
if (!ap.auto_armed || !motors.get_interlock() || ap.land_complete) {
@ -370,7 +370,7 @@ void Copter::guided_vel_control_run()
// guided_posvel_control_run - runs the guided spline controller
// called from guided_run
void Copter::guided_posvel_control_run()
void Sub::guided_posvel_control_run()
{
// if not auto armed or motors not enabled set throttle to zero and exit immediately
if (!ap.auto_armed || !motors.get_interlock() || ap.land_complete) {
@ -439,7 +439,7 @@ void Copter::guided_posvel_control_run()
// guided_angle_control_run - runs the guided angle controller
// called from guided_run
void Copter::guided_angle_control_run()
void Sub::guided_angle_control_run()
{
// if not auto armed or motors not enabled set throttle to zero and exit immediately
if (!ap.auto_armed || !motors.get_interlock() || ap.land_complete) {
@ -491,7 +491,7 @@ void Copter::guided_angle_control_run()
// Guided Limit code
// guided_limit_clear - clear/turn off guided limits
void Copter::guided_limit_clear()
void Sub::guided_limit_clear()
{
guided_limit.timeout_ms = 0;
guided_limit.alt_min_cm = 0.0f;
@ -500,7 +500,7 @@ void Copter::guided_limit_clear()
}
// guided_limit_set - set guided timeout and movement limits
void Copter::guided_limit_set(uint32_t timeout_ms, float alt_min_cm, float alt_max_cm, float horiz_max_cm)
void Sub::guided_limit_set(uint32_t timeout_ms, float alt_min_cm, float alt_max_cm, float horiz_max_cm)
{
guided_limit.timeout_ms = timeout_ms;
guided_limit.alt_min_cm = alt_min_cm;
@ -510,7 +510,7 @@ void Copter::guided_limit_set(uint32_t timeout_ms, float alt_min_cm, float alt_m
// guided_limit_init_time_and_pos - initialise guided start time and position as reference for limit checking
// only called from AUTO mode's auto_nav_guided_start function
void Copter::guided_limit_init_time_and_pos()
void Sub::guided_limit_init_time_and_pos()
{
// initialise start time
guided_limit.start_time = AP_HAL::millis();
@ -521,7 +521,7 @@ void Copter::guided_limit_init_time_and_pos()
// guided_limit_check - returns true if guided mode has breached a limit
// used when guided is invoked from the NAV_GUIDED_ENABLE mission command
bool Copter::guided_limit_check()
bool Sub::guided_limit_check()
{
// check if we have passed the timeout
if ((guided_limit.timeout_ms > 0) && (millis() - guided_limit.start_time >= guided_limit.timeout_ms)) {

18
ArduSub/control_land.cpp
View File

@ -1,6 +1,6 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#include "Copter.h"
#include "Sub.h"
static bool land_with_gps;
@ -8,7 +8,7 @@ static uint32_t land_start_time;
static bool land_pause;
// land_init - initialise land controller
bool Copter::land_init(bool ignore_checks)
bool Sub::land_init(bool ignore_checks)
{
// check if we have GPS and decide which LAND we're going to do
land_with_gps = position_ok();
@ -38,7 +38,7 @@ bool Copter::land_init(bool ignore_checks)
// land_run - runs the land controller
// should be called at 100hz or more
void Copter::land_run()
void Sub::land_run()
{
if (land_with_gps) {
land_gps_run();
@ -50,7 +50,7 @@ void Copter::land_run()
// land_run - runs the land controller
// horizontal position controlled with loiter controller
// should be called at 100hz or more
void Copter::land_gps_run()
void Sub::land_gps_run()
{
int16_t roll_control = 0, pitch_control = 0;
float target_yaw_rate = 0;
@ -141,7 +141,7 @@ void Copter::land_gps_run()
// land_nogps_run - runs the land controller
// pilot controls roll and pitch angles
// should be called at 100hz or more
void Copter::land_nogps_run()
void Sub::land_nogps_run()
{
float target_roll = 0.0f, target_pitch = 0.0f;
float target_yaw_rate = 0;
@ -207,7 +207,7 @@ void Copter::land_nogps_run()
// get_land_descent_speed - high level landing logic
// returns climb rate (in cm/s) which should be passed to the position controller
// should be called at 100hz or higher
float Copter::get_land_descent_speed()
float Sub::get_land_descent_speed()
{
#if CONFIG_SONAR == ENABLED
bool sonar_ok = sonar_enabled && (sonar.status() == RangeFinder::RangeFinder_Good);
@ -225,14 +225,14 @@ float Copter::get_land_descent_speed()
// land_do_not_use_GPS - forces land-mode to not use the GPS but instead rely on pilot input for roll and pitch
// called during GPS failsafe to ensure that if we were already in LAND mode that we do not use the GPS
// has no effect if we are not already in LAND mode
void Copter::land_do_not_use_GPS()
void Sub::land_do_not_use_GPS()
{
land_with_gps = false;
}
// set_mode_land_with_pause - sets mode to LAND and triggers 4 second delay before descent starts
// this is always called from a failsafe so we trigger notification to pilot
void Copter::set_mode_land_with_pause()
void Sub::set_mode_land_with_pause()
{
set_mode(LAND);
land_pause = true;
@ -242,6 +242,6 @@ void Copter::set_mode_land_with_pause()
}
// landing_with_GPS - returns true if vehicle is landing using GPS
bool Copter::landing_with_GPS() {
bool Sub::landing_with_GPS() {
return (control_mode == LAND && land_with_gps);
}

6
ArduSub/control_loiter.cpp
View File

@ -1,13 +1,13 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#include "Copter.h"
#include "Sub.h"
/*
* control_loiter.pde - init and run calls for loiter flight mode
*/
// loiter_init - initialise loiter controller
bool Copter::loiter_init(bool ignore_checks)
bool Sub::loiter_init(bool ignore_checks)
{
#if FRAME_CONFIG == HELI_FRAME
// do not allow helis to enter Loiter if the Rotor Runup is not complete
@ -37,7 +37,7 @@ bool Copter::loiter_init(bool ignore_checks)
// loiter_run - runs the loiter controller
// should be called at 100hz or more
void Copter::loiter_run()
void Sub::loiter_run()
{
LoiterModeState loiter_state;
float target_yaw_rate = 0.0f;

2
ArduSub/control_poshold.cpp
View File

@ -1,6 +1,6 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#include "Copter.h"
#include "Sub.h"
#if POSHOLD_ENABLED == ENABLED

6
ArduSub/control_rov.cpp
View File

@ -1,13 +1,13 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#include "Copter.h"
#include "Sub.h"
/*
* control_rov.cpp - Control for basic ROV operation
*/
// stabilize_init - initialise stabilize controller
bool Copter::rov_init(bool ignore_checks)
bool Sub::rov_init(bool ignore_checks)
{
// if landed and the mode we're switching from does not have manual throttle and the throttle stick is too high
//if (motors.armed() && ap.land_complete && !mode_has_manual_throttle(control_mode) && (g.rc_3.control_in > get_non_takeoff_throttle())) {
@ -21,7 +21,7 @@ bool Copter::rov_init(bool ignore_checks)
// stabilize_run - runs the main stabilize controller
// should be called at 100hz or more
void Copter::rov_run()
void Sub::rov_run()
{
float target_roll, target_pitch;
float target_yaw_rate;

26
ArduSub/control_rtl.cpp
View File

@ -1,6 +1,6 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#include "Copter.h"
#include "Sub.h"
/*
* control_rtl.pde - init and run calls for RTL flight mode
@ -10,7 +10,7 @@
*/
// rtl_init - initialise rtl controller
bool Copter::rtl_init(bool ignore_checks)
bool Sub::rtl_init(bool ignore_checks)
{
if (position_ok() || ignore_checks) {
rtl_climb_start();
@ -22,7 +22,7 @@ bool Copter::rtl_init(bool ignore_checks)
// rtl_run - runs the return-to-launch controller
// should be called at 100hz or more
void Copter::rtl_run()
void Sub::rtl_run()
{
// check if we need to move to next state
if (rtl_state_complete) {
@ -75,7 +75,7 @@ void Copter::rtl_run()
}
// rtl_climb_start - initialise climb to RTL altitude
void Copter::rtl_climb_start()
void Sub::rtl_climb_start()
{
rtl_state = RTL_InitialClimb;
rtl_state_complete = false;
@ -112,7 +112,7 @@ void Copter::rtl_climb_start()
}
// rtl_return_start - initialise return to home
void Copter::rtl_return_start()
void Sub::rtl_return_start()
{
rtl_state = RTL_ReturnHome;
rtl_state_complete = false;
@ -137,7 +137,7 @@ void Copter::rtl_return_start()
// rtl_climb_return_run - implements the initial climb, return home and descent portions of RTL which all rely on the wp controller
// called by rtl_run at 100hz or more
void Copter::rtl_climb_return_run()
void Sub::rtl_climb_return_run()
{
// if not auto armed or motor interlock not enabled set throttle to zero and exit immediately
if(!ap.auto_armed || !motors.get_interlock()) {
@ -183,7 +183,7 @@ void Copter::rtl_climb_return_run()
}
// rtl_return_start - initialise return to home
void Copter::rtl_loiterathome_start()
void Sub::rtl_loiterathome_start()
{
rtl_state = RTL_LoiterAtHome;
rtl_state_complete = false;
@ -199,7 +199,7 @@ void Copter::rtl_loiterathome_start()
// rtl_climb_return_descent_run - implements the initial climb, return home and descent portions of RTL which all rely on the wp controller
// called by rtl_run at 100hz or more
void Copter::rtl_loiterathome_run()
void Sub::rtl_loiterathome_run()
{
// if not auto armed or motor interlock not enabled set throttle to zero and exit immediately
if(!ap.auto_armed || !motors.get_interlock()) {
@ -255,7 +255,7 @@ void Copter::rtl_loiterathome_run()
}
// rtl_descent_start - initialise descent to final alt
void Copter::rtl_descent_start()
void Sub::rtl_descent_start()
{
rtl_state = RTL_FinalDescent;
rtl_state_complete = false;
@ -272,7 +272,7 @@ void Copter::rtl_descent_start()
// rtl_descent_run - implements the final descent to the RTL_ALT
// called by rtl_run at 100hz or more
void Copter::rtl_descent_run()
void Sub::rtl_descent_run()
{
int16_t roll_control = 0, pitch_control = 0;
float target_yaw_rate = 0;
@ -324,7 +324,7 @@ void Copter::rtl_descent_run()
}
// rtl_loiterathome_start - initialise controllers to loiter over home
void Copter::rtl_land_start()
void Sub::rtl_land_start()
{
rtl_state = RTL_Land;
rtl_state_complete = false;
@ -341,7 +341,7 @@ void Copter::rtl_land_start()
// rtl_returnhome_run - return home
// called by rtl_run at 100hz or more
void Copter::rtl_land_run()
void Sub::rtl_land_run()
{
int16_t roll_control = 0, pitch_control = 0;
float target_yaw_rate = 0;
@ -417,7 +417,7 @@ void Copter::rtl_land_run()
// get_RTL_alt - return altitude which vehicle should return home at
// altitude is in cm above home
float Copter::get_RTL_alt()
float Sub::get_RTL_alt()
{
// maximum of current altitude + climb_min and rtl altitude
float ret = MAX(current_loc.alt + MAX(0, g.rtl_climb_min), g.rtl_altitude);

6
ArduSub/control_sport.cpp
View File

@ -1,13 +1,13 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#include "Copter.h"
#include "Sub.h"
/*
* control_sport.pde - init and run calls for sport flight mode
*/
// sport_init - initialise sport controller
bool Copter::sport_init(bool ignore_checks)
bool Sub::sport_init(bool ignore_checks)
{
// initialize vertical speed and acceleration
pos_control.set_speed_z(-g.pilot_velocity_z_max, g.pilot_velocity_z_max);
@ -22,7 +22,7 @@ bool Copter::sport_init(bool ignore_checks)
// sport_run - runs the sport controller
// should be called at 100hz or more
void Copter::sport_run()
void Sub::sport_run()
{
float target_roll_rate, target_pitch_rate, target_yaw_rate;
float target_climb_rate = 0;

6
ArduSub/control_stabilize.cpp
View File

@ -1,13 +1,13 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#include "Copter.h"
#include "Sub.h"
/*
* control_stabilize.pde - init and run calls for stabilize flight mode
*/
// stabilize_init - initialise stabilize controller
bool Copter::stabilize_init(bool ignore_checks)
bool Sub::stabilize_init(bool ignore_checks)
{
// if landed and the mode we're switching from does not have manual throttle and the throttle stick is too high
if (motors.armed() && ap.land_complete && !mode_has_manual_throttle(control_mode) && (g.rc_3.control_in > get_non_takeoff_throttle())) {
@ -21,7 +21,7 @@ bool Copter::stabilize_init(bool ignore_checks)
// stabilize_run - runs the main stabilize controller
// should be called at 100hz or more
void Copter::stabilize_run()
void Sub::stabilize_run()
{
float target_roll, target_pitch;
float target_yaw_rate;

10
ArduSub/crash_check.cpp
View File

@ -1,6 +1,6 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#include "Copter.h"
#include "Sub.h"
// Code to detect a crash main ArduCopter code
#define CRASH_CHECK_TRIGGER_SEC 2 // 2 seconds inverted indicates a crash
@ -10,7 +10,7 @@
// crash_check - disarms motors if a crash has been detected
// crashes are detected by the vehicle being more than 20 degrees beyond it's angle limits continuously for more than 1 second
// called at MAIN_LOOP_RATE
void Copter::crash_check()
void Sub::crash_check()
{
static uint16_t crash_counter; // number of iterations vehicle may have been crashed
@ -62,7 +62,7 @@ void Copter::crash_check()
// parachute_check - disarms motors and triggers the parachute if serious loss of control has been detected
// vehicle is considered to have a "serious loss of control" by the vehicle being more than 30 degrees off from the target roll and pitch angles continuously for 1 second
// called at MAIN_LOOP_RATE
void Copter::parachute_check()
void Sub::parachute_check()
{
static uint16_t control_loss_count; // number of iterations we have been out of control
static int32_t baro_alt_start;
@ -133,7 +133,7 @@ void Copter::parachute_check()
}
// parachute_release - trigger the release of the parachute, disarm the motors and notify the user
void Copter::parachute_release()
void Sub::parachute_release()
{
// send message to gcs and dataflash
gcs_send_text(MAV_SEVERITY_INFO,"Parachute: Released");
@ -148,7 +148,7 @@ void Copter::parachute_release()
// parachute_manual_release - trigger the release of the parachute, after performing some checks for pilot error
// checks if the vehicle is landed
void Copter::parachute_manual_release()
void Sub::parachute_manual_release()
{
// exit immediately if parachute is not enabled
if (!parachute.enabled()) {

10
ArduSub/ekf_check.cpp
View File

@ -1,6 +1,6 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#include "Copter.h"
#include "Sub.h"
/**
*
@ -28,7 +28,7 @@ static struct {
// ekf_check - detects if ekf variance are out of tolerance and triggers failsafe
// should be called at 10hz
void Copter::ekf_check()
void Sub::ekf_check()
{
// exit immediately if ekf has no origin yet - this assumes the origin can never become unset
Location temp_loc;
@ -89,7 +89,7 @@ void Copter::ekf_check()
}
// ekf_over_threshold - returns true if the ekf's variance are over the tolerance
bool Copter::ekf_over_threshold()
bool Sub::ekf_over_threshold()
{
// return false immediately if disabled
if (g.fs_ekf_thresh <= 0.0f) {
@ -116,7 +116,7 @@ bool Copter::ekf_over_threshold()
// failsafe_ekf_event - perform ekf failsafe
void Copter::failsafe_ekf_event()
void Sub::failsafe_ekf_event()
{
// return immediately if ekf failsafe already triggered
if (failsafe.ekf) {
@ -157,7 +157,7 @@ void Copter::failsafe_ekf_event()
}
// failsafe_ekf_off_event - actions to take when EKF failsafe is cleared
void Copter::failsafe_ekf_off_event(void)
void Sub::failsafe_ekf_off_event(void)
{
// return immediately if not in ekf failsafe
if (!failsafe.ekf) {

8
ArduSub/esc_calibration.cpp
View File

@ -1,6 +1,6 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#include "Copter.h"
#include "Sub.h"
/*****************************************************************************
* esc_calibration.pde : functions to check and perform ESC calibration
@ -18,7 +18,7 @@ enum ESCCalibrationModes {
};
// check if we should enter esc calibration mode
void Copter::esc_calibration_startup_check()
void Sub::esc_calibration_startup_check()
{
#if FRAME_CONFIG != HELI_FRAME
// exit immediately if pre-arm rc checks fail
@ -75,7 +75,7 @@ void Copter::esc_calibration_startup_check()
}
// esc_calibration_passthrough - pass through pilot throttle to escs
void Copter::esc_calibration_passthrough()
void Sub::esc_calibration_passthrough()
{
#if FRAME_CONFIG != HELI_FRAME
// clear esc flag for next time
@ -106,7 +106,7 @@ void Copter::esc_calibration_passthrough()
}
// esc_calibration_auto - calibrate the ESCs automatically using a timer and no pilot input
void Copter::esc_calibration_auto()
void Sub::esc_calibration_auto()
{
#if FRAME_CONFIG != HELI_FRAME
bool printed_msg = false;

16
ArduSub/events.cpp
View File

@ -1,12 +1,12 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#include "Copter.h"
#include "Sub.h"
/*
* This event will be called when the failsafe changes
* boolean failsafe reflects the current state
*/
void Copter::failsafe_radio_on_event()
void Sub::failsafe_radio_on_event()
{
// if motors are not armed there is nothing to do
if( !motors.armed() ) {
@ -93,14 +93,14 @@ void Copter::failsafe_radio_on_event()
// failsafe_off_event - respond to radio contact being regained
// we must be in AUTO, LAND or RTL modes
// or Stabilize or ACRO mode but with motors disarmed
void Copter::failsafe_radio_off_event()
void Sub::failsafe_radio_off_event()
{
// no need to do anything except log the error as resolved
// user can now override roll, pitch, yaw and throttle and even use flight mode switch to restore previous flight mode
Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE_RADIO, ERROR_CODE_FAILSAFE_RESOLVED);
}
void Copter::failsafe_battery_event(void)
void Sub::failsafe_battery_event(void)
{
// return immediately if low battery event has already been triggered
if (failsafe.battery) {
@ -165,7 +165,7 @@ void Copter::failsafe_battery_event(void)
}
// failsafe_gcs_check - check for ground station failsafe
void Copter::failsafe_gcs_check()
void Sub::failsafe_gcs_check()
{
uint32_t last_gcs_update_ms;
@ -253,7 +253,7 @@ void Copter::failsafe_gcs_check()
}
// failsafe_gcs_off_event - actions to take when GCS contact is restored
void Copter::failsafe_gcs_off_event(void)
void Sub::failsafe_gcs_off_event(void)
{
// log recovery of GCS in logs?
Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE_GCS, ERROR_CODE_FAILSAFE_RESOLVED);
@ -261,7 +261,7 @@ void Copter::failsafe_gcs_off_event(void)
// set_mode_RTL_or_land_with_pause - sets mode to RTL if possible or LAND with 4 second delay before descent starts
// this is always called from a failsafe so we trigger notification to pilot
void Copter::set_mode_RTL_or_land_with_pause()
void Sub::set_mode_RTL_or_land_with_pause()
{
// attempt to switch to RTL, if this fails then switch to Land
if (!set_mode(RTL)) {
@ -273,7 +273,7 @@ void Copter::set_mode_RTL_or_land_with_pause()
}
}
void Copter::update_events()
void Sub::update_events()
{
ServoRelayEvents.update_events();
}

8
ArduSub/failsafe.cpp
View File

@ -1,6 +1,6 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#include "Copter.h"
#include "Sub.h"
//
// failsafe support
@ -17,7 +17,7 @@ static bool in_failsafe;
//
// failsafe_enable - enable failsafe
//
void Copter::failsafe_enable()
void Sub::failsafe_enable()
{
failsafe_enabled = true;
failsafe_last_timestamp = micros();
@ -26,7 +26,7 @@ void Copter::failsafe_enable()
//
// failsafe_disable - used when we know we are going to delay the mainloop significantly
//
void Copter::failsafe_disable()
void Sub::failsafe_disable()
{
failsafe_enabled = false;
}
@ -34,7 +34,7 @@ void Copter::failsafe_disable()
//
// failsafe_check - this function is called from the core timer interrupt at 1kHz.
//
void Copter::failsafe_check()
void Sub::failsafe_check()
{
uint32_t tnow = AP_HAL::micros();

2
ArduSub/fence.cpp
View File

@ -1,6 +1,6 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#include "Copter.h"
#include "Sub.h"
// Code to integrate AC_Fence library with main ArduCopter code

18
ArduSub/flight_mode.cpp
View File

@ -1,6 +1,6 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#include "Copter.h"
#include "Sub.h"
/*
* flight.pde - high level calls to set and update flight modes
@ -11,7 +11,7 @@
// optional force parameter used to force the flight mode change (used only first time mode is set)
// returns true if mode was succesfully set
// ACRO, STABILIZE, ALTHOLD, LAND, DRIFT and SPORT can always be set successfully but the return state of other flight modes should be checked and the caller should deal with failures appropriately
bool Copter::set_mode(uint8_t mode)
bool Sub::set_mode(uint8_t mode)
{
// boolean to record if flight mode could be set
bool success = false;
@ -129,7 +129,7 @@ bool Copter::set_mode(uint8_t mode)
// update_flight_mode - calls the appropriate attitude controllers based on flight mode
// called at 100hz or more
void Copter::update_flight_mode()
void Sub::update_flight_mode()
{
// Update EKF speed limit - used to limit speed when we are using optical flow
ahrs.getEkfControlLimits(ekfGndSpdLimit, ekfNavVelGainScaler);
@ -210,7 +210,7 @@ void Copter::update_flight_mode()
}
// exit_mode - high level call to organise cleanup as a flight mode is exited
void Copter::exit_mode(uint8_t old_control_mode, uint8_t new_control_mode)
void Sub::exit_mode(uint8_t old_control_mode, uint8_t new_control_mode)
{
#if AUTOTUNE_ENABLED == ENABLED
if (old_control_mode == AUTOTUNE) {
@ -261,7 +261,7 @@ void Copter::exit_mode(uint8_t old_control_mode, uint8_t new_control_mode)
}
// returns true or false whether mode requires GPS
bool Copter::mode_requires_GPS(uint8_t mode) {
bool Sub::mode_requires_GPS(uint8_t mode) {
switch(mode) {
case AUTO:
case GUIDED:
@ -280,7 +280,7 @@ bool Copter::mode_requires_GPS(uint8_t mode) {
}
// mode_has_manual_throttle - returns true if the flight mode has a manual throttle (i.e. pilot directly controls throttle)
bool Copter::mode_has_manual_throttle(uint8_t mode) {
bool Sub::mode_has_manual_throttle(uint8_t mode) {
switch(mode) {
case ACRO:
case STABILIZE:
@ -294,7 +294,7 @@ bool Copter::mode_has_manual_throttle(uint8_t mode) {
// mode_allows_arming - returns true if vehicle can be armed in the specified mode
// arming_from_gcs should be set to true if the arming request comes from the ground station
bool Copter::mode_allows_arming(uint8_t mode, bool arming_from_gcs) {
bool Sub::mode_allows_arming(uint8_t mode, bool arming_from_gcs) {
if (mode_has_manual_throttle(mode) || mode == LOITER || mode == ALT_HOLD || mode == POSHOLD || (arming_from_gcs && mode == GUIDED)) {
return true;
}
@ -302,7 +302,7 @@ bool Copter::mode_allows_arming(uint8_t mode, bool arming_from_gcs) {
}
// notify_flight_mode - sets notify object based on flight mode. Only used for OreoLED notify device
void Copter::notify_flight_mode(uint8_t mode) {
void Sub::notify_flight_mode(uint8_t mode) {
switch(mode) {
case AUTO:
case GUIDED:
@ -322,7 +322,7 @@ void Copter::notify_flight_mode(uint8_t mode) {
//
// print_flight_mode - prints flight mode to serial port.
//
void Copter::print_flight_mode(AP_HAL::BetterStream *port, uint8_t mode)
void Sub::print_flight_mode(AP_HAL::BetterStream *port, uint8_t mode)
{
switch (mode) {
case STABILIZE:

2
ArduSub/heli.cpp
View File

@ -1,6 +1,6 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#include "Copter.h"
#include "Sub.h"
// Traditional helicopter variables and functions

2
ArduSub/heli_control_acro.cpp
View File

@ -1,6 +1,6 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#include "Copter.h"
#include "Sub.h"
#if FRAME_CONFIG == HELI_FRAME
/*

2
ArduSub/heli_control_stabilize.cpp
View File

@ -1,6 +1,6 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#include "Copter.h"
#include "Sub.h"
#if FRAME_CONFIG == HELI_FRAME
/*

6
ArduSub/inertia.cpp
View File

@ -1,16 +1,16 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#include "Copter.h"
#include "Sub.h"
// read_inertia - read inertia in from accelerometers
void Copter::read_inertia()
void Sub::read_inertia()
{
// inertial altitude estimates
inertial_nav.update(G_Dt);
}
// read_inertial_altitude - pull altitude and climb rate from inertial nav library
void Copter::read_inertial_altitude()
void Sub::read_inertial_altitude()
{
// exit immediately if we do not have an altitude estimate
if (!inertial_nav.get_filter_status().flags.vert_pos) {

12
ArduSub/land_detector.cpp
View File

@ -1,6 +1,6 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#include "Copter.h"
#include "Sub.h"
// counter to verify landings
@ -8,7 +8,7 @@ static uint32_t land_detector_count = 0;
// run land and crash detectors
// called at MAIN_LOOP_RATE
void Copter::update_land_and_crash_detectors()
void Sub::update_land_and_crash_detectors()
{
// update 1hz filtered acceleration
Vector3f accel_ef = ahrs.get_accel_ef_blended();
@ -27,7 +27,7 @@ void Copter::update_land_and_crash_detectors()
// update_land_detector - checks if we have landed and updates the ap.land_complete flag
// called at MAIN_LOOP_RATE
void Copter::update_land_detector()
void Sub::update_land_detector()
{
// land detector can not use the following sensors because they are unreliable during landing
// barometer altitude : ground effect can cause errors larger than 4m
@ -79,7 +79,7 @@ void Copter::update_land_detector()
set_land_complete_maybe(ap.land_complete || (land_detector_count >= LAND_DETECTOR_MAYBE_TRIGGER_SEC*MAIN_LOOP_RATE));
}
void Copter::set_land_complete(bool b)
void Sub::set_land_complete(bool b)
{
// if no change, exit immediately
if( ap.land_complete == b )
@ -96,7 +96,7 @@ void Copter::set_land_complete(bool b)
}
// set land complete maybe flag
void Copter::set_land_complete_maybe(bool b)
void Sub::set_land_complete_maybe(bool b)
{
// if no change, exit immediately
if (ap.land_complete_maybe == b)
@ -111,7 +111,7 @@ void Copter::set_land_complete_maybe(bool b)
// update_throttle_thr_mix - sets motors throttle_low_comp value depending upon vehicle state
// low values favour pilot/autopilot throttle over attitude control, high values favour attitude control over throttle
// has no effect when throttle is above hover throttle
void Copter::update_throttle_thr_mix()
void Sub::update_throttle_thr_mix()
{
#if FRAME_CONFIG != HELI_FRAME
// if disarmed or landed prioritise throttle

4
ArduSub/landing_gear.cpp
View File

@ -1,10 +1,10 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#include "Copter.h"
#include "Sub.h"
// Run landing gear controller at 10Hz
void Copter::landinggear_update(){
void Sub::landinggear_update(){
// If landing gear control is active, run update function.
if (check_if_auxsw_mode_used(AUXSW_LANDING_GEAR)){

4
ArduSub/leds.cpp
View File

@ -1,11 +1,11 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#include "Copter.h"
#include "Sub.h"
// updates the status of notify
// should be called at 50hz
void Copter::update_notify()
void Sub::update_notify()
{
notify.update();
}

10
ArduSub/motor_test.cpp
View File

@ -1,6 +1,6 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#include "Copter.h"
#include "Sub.h"
/*
mavlink motor test - implements the MAV_CMD_DO_MOTOR_TEST mavlink command so that the GCS/pilot can test an individual motor or flaps
@ -19,7 +19,7 @@ static uint8_t motor_test_throttle_type = 0; // motor throttle type (0=thrott
static uint16_t motor_test_throttle_value = 0; // throttle to be sent to motor, value depends upon it's type
// motor_test_output - checks for timeout and sends updates to motors objects
void Copter::motor_test_output()
void Sub::motor_test_output()
{
// exit immediately if the motor test is not running
if (!ap.motor_test) {
@ -69,7 +69,7 @@ void Copter::motor_test_output()
// mavlink_motor_test_check - perform checks before motor tests can begin
// return true if tests can continue, false if not
bool Copter::mavlink_motor_test_check(mavlink_channel_t chan, bool check_rc)
bool Sub::mavlink_motor_test_check(mavlink_channel_t chan, bool check_rc)
{
// check rc has been calibrated
pre_arm_rc_checks();
@ -96,7 +96,7 @@ bool Copter::mavlink_motor_test_check(mavlink_channel_t chan, bool check_rc)
// mavlink_motor_test_start - start motor test - spin a single motor at a specified pwm
// returns MAV_RESULT_ACCEPTED on success, MAV_RESULT_FAILED on failure
uint8_t Copter::mavlink_motor_test_start(mavlink_channel_t chan, uint8_t motor_seq, uint8_t throttle_type, uint16_t throttle_value, float timeout_sec)
uint8_t Sub::mavlink_motor_test_start(mavlink_channel_t chan, uint8_t motor_seq, uint8_t throttle_type, uint16_t throttle_value, float timeout_sec)
{
// if test has not started try to start it
if (!ap.motor_test) {
@ -141,7 +141,7 @@ uint8_t Copter::mavlink_motor_test_start(mavlink_channel_t chan, uint8_t motor_s
}
// motor_test_stop - stops the motor test
void Copter::motor_test_stop()
void Sub::motor_test_stop()
{
// exit immediately if the test is not running
if (!ap.motor_test) {

14
ArduSub/motors.cpp
View File

@ -1,6 +1,6 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#include "Copter.h"
#include "Sub.h"
#define ARM_DELAY 20 // called at 10hz so 2 seconds
#define DISARM_DELAY 20 // called at 10hz so 2 seconds
@ -11,7 +11,7 @@ static uint32_t auto_disarm_begin;
// arm_motors_check - checks for pilot input to arm or disarm the copter
// called at 10hz
void Copter::arm_motors_check()
void Sub::arm_motors_check()
{
<<<<<<< 6dafedb2d1ad5061d859a9c319fa4b69b4ac5dd9
// Arm motors automatically
@ -83,7 +83,7 @@ void Copter::arm_motors_check()
}
// auto_disarm_check - disarms the copter if it has been sitting on the ground in manual mode with throttle low for at least 15 seconds
void Copter::auto_disarm_check()
void Sub::auto_disarm_check()
{
<<<<<<< 6dafedb2d1ad5061d859a9c319fa4b69b4ac5dd9
/*uint32_t tnow_ms = millis();
@ -143,7 +143,7 @@ void Copter::auto_disarm_check()
// init_arm_motors - performs arming process including initialisation of barometer and gyros
// returns false if arming failed because of pre-arm checks, arming checks or a gyro calibration failure
bool Copter::init_arm_motors(bool arming_from_gcs)
bool Sub::init_arm_motors(bool arming_from_gcs)
{
static bool in_arm_motors = false;
@ -231,7 +231,7 @@ bool Copter::init_arm_motors(bool arming_from_gcs)
}
// init_disarm_motors - disarm motors
void Copter::init_disarm_motors()
void Sub::init_disarm_motors()
{
// return immediately if we are already disarmed
if (!motors.armed()) {
@ -277,7 +277,7 @@ void Copter::init_disarm_motors()
}
// motors_output - send output to motors library which will adjust and send to ESCs and servos
void Copter::motors_output()
void Sub::motors_output()
{
// check if we are performing the motor test
if (ap.motor_test) {
@ -295,7 +295,7 @@ void Copter::motors_output()
}
// check for pilot stick input to trigger lost vehicle alarm
void Copter::lost_vehicle_check()
void Sub::lost_vehicle_check()
{
static uint8_t soundalarm_counter;

16
ArduSub/navigation.cpp
View File

@ -1,11 +1,11 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#include "Copter.h"
#include "Sub.h"
// run_nav_updates - top level call for the autopilot
// ensures calculations such as "distance to waypoint" are calculated before autopilot makes decisions
// To-Do - rename and move this function to make it's purpose more clear
void Copter::run_nav_updates(void)
void Sub::run_nav_updates(void)
{
// fetch position from inertial navigation
calc_position();
@ -18,7 +18,7 @@ void Copter::run_nav_updates(void)
}
// calc_position - get lat and lon positions from inertial nav library
void Copter::calc_position()
void Sub::calc_position()
{
// pull position from interial nav library
current_loc.lng = inertial_nav.get_longitude();
@ -26,7 +26,7 @@ void Copter::calc_position()
}
// calc_distance_and_bearing - calculate distance and bearing to next waypoint and home
void Copter::calc_distance_and_bearing()
void Sub::calc_distance_and_bearing()
{
calc_wp_distance();
calc_wp_bearing();
@ -34,7 +34,7 @@ void Copter::calc_distance_and_bearing()
}
// calc_wp_distance - calculate distance to next waypoint for reporting and autopilot decisions
void Copter::calc_wp_distance()
void Sub::calc_wp_distance()
{
// get target from loiter or wpinav controller
if (control_mode == LOITER || control_mode == CIRCLE) {
@ -47,7 +47,7 @@ void Copter::calc_wp_distance()
}
// calc_wp_bearing - calculate bearing to next waypoint for reporting and autopilot decisions
void Copter::calc_wp_bearing()
void Sub::calc_wp_bearing()
{
// get target from loiter or wpinav controller
if (control_mode == LOITER || control_mode == CIRCLE) {
@ -60,7 +60,7 @@ void Copter::calc_wp_bearing()
}
// calc_home_distance_and_bearing - calculate distance and bearing to home for reporting and autopilot decisions
void Copter::calc_home_distance_and_bearing()
void Sub::calc_home_distance_and_bearing()
{
// calculate home distance and bearing
if (position_ok()) {
@ -75,7 +75,7 @@ void Copter::calc_home_distance_and_bearing()
}
// run_autopilot - highest level call to process mission commands
void Copter::run_autopilot()
void Sub::run_autopilot()
{
if (control_mode == AUTO) {
// update state of mission

16
ArduSub/perf_info.cpp
View File

@ -1,6 +1,6 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#include "Copter.h"
#include "Sub.h"
//
// high level performance monitoring
@ -18,7 +18,7 @@ static uint16_t perf_info_long_running;
static bool perf_ignore_loop = false;
// perf_info_reset - reset all records of loop time to zero
void Copter::perf_info_reset()
void Sub::perf_info_reset()
{
perf_info_loop_count = 0;
perf_info_max_time = 0;
@ -27,13 +27,13 @@ void Copter::perf_info_reset()
}
// perf_ignore_loop - ignore this loop from performance measurements (used to reduce false positive when arming)
void Copter::perf_ignore_this_loop()
void Sub::perf_ignore_this_loop()
{
perf_ignore_loop = true;
}
// perf_info_check_loop_time - check latest loop time vs min, max and overtime threshold
void Copter::perf_info_check_loop_time(uint32_t time_in_micros)
void Sub::perf_info_check_loop_time(uint32_t time_in_micros)
{
perf_info_loop_count++;
@ -55,25 +55,25 @@ void Copter::perf_info_check_loop_time(uint32_t time_in_micros)
}
// perf_info_get_long_running_percentage - get number of long running loops as a percentage of the total number of loops
uint16_t Copter::perf_info_get_num_loops()
uint16_t Sub::perf_info_get_num_loops()
{
return perf_info_loop_count;
}
// perf_info_get_max_time - return maximum loop time (in microseconds)
uint32_t Copter::perf_info_get_max_time()
uint32_t Sub::perf_info_get_max_time()
{
return perf_info_max_time;
}
// perf_info_get_max_time - return maximum loop time (in microseconds)
uint32_t Copter::perf_info_get_min_time()
uint32_t Sub::perf_info_get_min_time()
{
return perf_info_min_time;
}
// perf_info_get_num_long_running - get number of long running loops
uint16_t Copter::perf_info_get_num_long_running()
uint16_t Sub::perf_info_get_num_long_running()
{
return perf_info_long_running;
}

14
ArduSub/position_vector.cpp
View File

@ -1,6 +1,6 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#include "Copter.h"
#include "Sub.h"
// position_vector.pde related utility functions
@ -10,7 +10,7 @@
// .z = altitude above home in cm
// pv_location_to_vector - convert lat/lon coordinates to a position vector
Vector3f Copter::pv_location_to_vector(const Location& loc)
Vector3f Sub::pv_location_to_vector(const Location& loc)
{
const struct Location &origin = inertial_nav.get_origin();
float alt_above_origin = pv_alt_above_origin(loc.alt); // convert alt-relative-to-home to alt-relative-to-origin
@ -19,7 +19,7 @@ Vector3f Copter::pv_location_to_vector(const Location& loc)
// pv_location_to_vector_with_default - convert lat/lon coordinates to a position vector,
// defaults to the default_posvec's lat/lon and alt if the provided lat/lon or alt are zero
Vector3f Copter::pv_location_to_vector_with_default(const Location& loc, const Vector3f& default_posvec)
Vector3f Sub::pv_location_to_vector_with_default(const Location& loc, const Vector3f& default_posvec)
{
Vector3f pos = pv_location_to_vector(loc);
@ -38,21 +38,21 @@ Vector3f Copter::pv_location_to_vector_with_default(const Location& loc, const V
}
// pv_alt_above_origin - convert altitude above home to altitude above EKF origin
float Copter::pv_alt_above_origin(float alt_above_home_cm)
float Sub::pv_alt_above_origin(float alt_above_home_cm)
{
const struct Location &origin = inertial_nav.get_origin();
return alt_above_home_cm + (ahrs.get_home().alt - origin.alt);
}
// pv_alt_above_home - convert altitude above EKF origin to altitude above home
float Copter::pv_alt_above_home(float alt_above_origin_cm)
float Sub::pv_alt_above_home(float alt_above_origin_cm)
{
const struct Location &origin = inertial_nav.get_origin();
return alt_above_origin_cm + (origin.alt - ahrs.get_home().alt);
}
// pv_get_bearing_cd - return bearing in centi-degrees between two positions
float Copter::pv_get_bearing_cd(const Vector3f &origin, const Vector3f &destination)
float Sub::pv_get_bearing_cd(const Vector3f &origin, const Vector3f &destination)
{
float bearing = atan2f(destination.y-origin.y, destination.x-origin.x) * DEGX100;
if (bearing < 0) {
@ -62,7 +62,7 @@ float Copter::pv_get_bearing_cd(const Vector3f &origin, const Vector3f &destinat
}
// pv_get_horizontal_distance_cm - return distance between two positions in cm
float Copter::pv_get_horizontal_distance_cm(const Vector3f &origin, const Vector3f &destination)
float Sub::pv_get_horizontal_distance_cm(const Vector3f &origin, const Vector3f &destination)
{
return pythagorous2(destination.x-origin.x,destination.y-origin.y);
}

2
ArduSub/precision_landing.cpp
View File

@ -4,7 +4,7 @@
// functions to support precision landing
//
#include "Copter.h"
#include "Sub.h"
#if PRECISION_LANDING == ENABLED

18
ArduSub/radio.cpp
View File

@ -1,12 +1,12 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#include "Copter.h"
#include "Sub.h"
// Function that will read the radio data, limit servos and trigger a failsafe
// ----------------------------------------------------------------------------
void Copter::default_dead_zones()
void Sub::default_dead_zones()
{
channel_roll->set_default_dead_zone(30);
channel_pitch->set_default_dead_zone(30);
@ -21,7 +21,7 @@ void Copter::default_dead_zones()
g.rc_6.set_default_dead_zone(0);
}
void Copter::init_rc_in()
void Sub::init_rc_in()
{
channel_roll = RC_Channel::rc_channel(rcmap.roll()-1);
channel_pitch = RC_Channel::rc_channel(rcmap.pitch()-1);
@ -67,7 +67,7 @@ void Copter::init_rc_in()
}
// init_rc_out -- initialise motors and check if pilot wants to perform ESC calibration
void Copter::init_rc_out()
void Sub::init_rc_out()
{
motors.set_update_rate(g.rc_speed);
motors.set_frame_orientation(g.frame_orientation);
@ -103,14 +103,14 @@ void Copter::init_rc_out()
}
// enable_motor_output() - enable and output lowest possible value to motors
void Copter::enable_motor_output()
void Sub::enable_motor_output()
{
// enable motors
motors.enable();
motors.output_min();
}
void Copter::read_radio()
void Sub::read_radio()
{
static uint32_t last_update_ms = 0;
uint32_t tnow_ms = millis();
@ -141,7 +141,7 @@ void Copter::read_radio()
}
}
void Copter::transform_manual_control_to_rc_override(int16_t x, int16_t y, int16_t z, int16_t r, uint16_t buttons) {
void Sub::transform_manual_control_to_rc_override(int16_t x, int16_t y, int16_t z, int16_t r, uint16_t buttons) {
int16_t channels[8];
float rpyScale = 0.5;
@ -172,7 +172,7 @@ void Copter::transform_manual_control_to_rc_override(int16_t x, int16_t y, int16
}
#define FS_COUNTER 3 // radio failsafe kicks in after 3 consecutive throttle values below failsafe_throttle_value
void Copter::set_throttle_and_failsafe(uint16_t throttle_pwm)
void Sub::set_throttle_and_failsafe(uint16_t throttle_pwm)
{
// if failsafe not enabled pass through throttle and exit
if(g.failsafe_throttle == FS_THR_DISABLED) {
@ -218,7 +218,7 @@ void Copter::set_throttle_and_failsafe(uint16_t throttle_pwm)
// throttle_zero is used to determine if the pilot intends to shut down the motors
// Basically, this signals when we are not flying. We are either on the ground
// or the pilot has shut down the copter in the air and it is free-falling
void Copter::set_throttle_zero_flag(int16_t throttle_control)
void Sub::set_throttle_zero_flag(int16_t throttle_control)
{
static uint32_t last_nonzero_throttle_ms = 0;
uint32_t tnow_ms = millis();

28
ArduSub/sensors.cpp
View File

@ -1,8 +1,8 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#include "Copter.h"
#include "Sub.h"
void Copter::init_barometer(bool full_calibration)
void Sub::init_barometer(bool full_calibration)
{
gcs_send_text(MAV_SEVERITY_INFO, "Calibrating barometer");
if (full_calibration) {
@ -14,7 +14,7 @@ void Copter::init_barometer(bool full_calibration)
}
// return barometric altitude in centimeters
void Copter::read_barometer(void)
void Sub::read_barometer(void)
{
barometer.update();
if (should_log(MASK_LOG_IMU)) {
@ -27,14 +27,14 @@ void Copter::read_barometer(void)
}
#if CONFIG_SONAR == ENABLED
void Copter::init_sonar(void)
void Sub::init_sonar(void)
{
sonar.init();
}
#endif
// return sonar altitude in centimeters
int16_t Copter::read_sonar(void)
int16_t Sub::read_sonar(void)
{
#if CONFIG_SONAR == ENABLED
sonar.update();
@ -72,7 +72,7 @@ int16_t Copter::read_sonar(void)
/*
update RPM sensors
*/
void Copter::rpm_update(void)
void Sub::rpm_update(void)
{
rpm_sensor.update();
if (rpm_sensor.enabled(0) || rpm_sensor.enabled(1)) {
@ -83,7 +83,7 @@ void Copter::rpm_update(void)
}
// initialise compass
void Copter::init_compass()
void Sub::init_compass()
{
if (!compass.init() || !compass.read()) {
// make sure we don't pass a broken compass to DCM
@ -95,7 +95,7 @@ void Copter::init_compass()
}
// initialise optical flow sensor
void Copter::init_optflow()
void Sub::init_optflow()
{
#if OPTFLOW == ENABLED
// exit immediately if not enabled
@ -110,7 +110,7 @@ void Copter::init_optflow()
// called at 200hz
#if OPTFLOW == ENABLED
void Copter::update_optical_flow(void)
void Sub::update_optical_flow(void)
{
static uint32_t last_of_update = 0;
@ -138,7 +138,7 @@ void Copter::update_optical_flow(void)
// read_battery - check battery voltage and current and invoke failsafe if necessary
// called at 10hz
void Copter::read_battery(void)
void Sub::read_battery(void)
{
battery.read();
@ -169,19 +169,19 @@ void Copter::read_battery(void)
// read the receiver RSSI as an 8 bit number for MAVLink
// RC_CHANNELS_SCALED message
void Copter::read_receiver_rssi(void)
void Sub::read_receiver_rssi(void)
{
receiver_rssi = rssi.read_receiver_rssi_uint8();
}
void Copter::compass_cal_update()
void Sub::compass_cal_update()
{
if (!hal.util->get_soft_armed()) {
compass.compass_cal_update();
}
}
void Copter::accel_cal_update()
void Sub::accel_cal_update()
{
if (hal.util->get_soft_armed()) {
return;
@ -196,7 +196,7 @@ void Copter::accel_cal_update()
#if EPM_ENABLED == ENABLED
// epm update - moves epm pwm output back to neutral after grab or release is completed
void Copter::epm_update()
void Sub::epm_update()
{
epm.update();
}

42
ArduSub/setup.cpp
View File

@ -1,6 +1,6 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#include "Copter.h"
#include "Sub.h"
#if CLI_ENABLED == ENABLED
@ -23,7 +23,7 @@ static const struct Menu::command setup_menu_commands[] = {
MENU(setup_menu, "setup", setup_menu_commands);
// Called from the top-level menu to run the setup menu.
int8_t Copter::setup_mode(uint8_t argc, const Menu::arg *argv)
int8_t Sub::setup_mode(uint8_t argc, const Menu::arg *argv)
{
// Give the user some guidance
cliSerial->printf("Setup Mode\n\n\n");
@ -35,7 +35,7 @@ int8_t Copter::setup_mode(uint8_t argc, const Menu::arg *argv)
// Initialise the EEPROM to 'factory' settings (mostly defined in APM_Config.h or via defaults).
// Called by the setup menu 'factoryreset' command.
int8_t Copter::setup_factory(uint8_t argc, const Menu::arg *argv)
int8_t Sub::setup_factory(uint8_t argc, const Menu::arg *argv)
{
int16_t c;
@ -61,7 +61,7 @@ int8_t Copter::setup_factory(uint8_t argc, const Menu::arg *argv)
//Set a parameter to a specified value. It will cast the value to the current type of the
//parameter and make sure it fits in case of INT8 and INT16
int8_t Copter::setup_set(uint8_t argc, const Menu::arg *argv)
int8_t Sub::setup_set(uint8_t argc, const Menu::arg *argv)
{
int8_t value_int8;
int16_t value_int16;
@ -120,7 +120,7 @@ int8_t Copter::setup_set(uint8_t argc, const Menu::arg *argv)
// Print the current configuration.
// Called by the setup menu 'show' command.
int8_t Copter::setup_show(uint8_t argc, const Menu::arg *argv)
int8_t Sub::setup_show(uint8_t argc, const Menu::arg *argv)
{
AP_Param *param;
ap_var_type type;
@ -157,7 +157,7 @@ int8_t Copter::setup_show(uint8_t argc, const Menu::arg *argv)
return(0);
}
int8_t Copter::esc_calib(uint8_t argc,const Menu::arg *argv)
int8_t Sub::esc_calib(uint8_t argc,const Menu::arg *argv)
{
@ -298,7 +298,7 @@ int8_t Copter::esc_calib(uint8_t argc,const Menu::arg *argv)
// CLI reports
/***************************************************************************/
void Copter::report_batt_monitor()
void Sub::report_batt_monitor()
{
cliSerial->printf("\nBatt Mon:\n");
print_divider();
@ -312,7 +312,7 @@ void Copter::report_batt_monitor()
print_blanks(2);
}
void Copter::report_frame()
void Sub::report_frame()
{
cliSerial->printf("Frame\n");
print_divider();
@ -334,7 +334,7 @@ void Copter::report_frame()
print_blanks(2);
}
void Copter::report_radio()
void Sub::report_radio()
{
cliSerial->printf("Radio\n");
print_divider();
@ -343,7 +343,7 @@ void Copter::report_radio()
print_blanks(2);
}
void Copter::report_ins()
void Sub::report_ins()
{
cliSerial->printf("INS\n");
print_divider();
@ -353,7 +353,7 @@ void Copter::report_ins()
print_blanks(2);
}
void Copter::report_flight_modes()
void Sub::report_flight_modes()
{
cliSerial->printf("Flight modes\n");
print_divider();
@ -364,7 +364,7 @@ void Copter::report_flight_modes()
print_blanks(2);
}
void Copter::report_optflow()
void Sub::report_optflow()
{
#if OPTFLOW == ENABLED
cliSerial->printf("OptFlow\n");
@ -380,7 +380,7 @@ void Copter::report_optflow()
// CLI utilities
/***************************************************************************/
void Copter::print_radio_values()
void Sub::print_radio_values()
{
cliSerial->printf("CH1: %d | %d\n", (int)channel_roll->radio_min, (int)channel_roll->radio_max);
cliSerial->printf("CH2: %d | %d\n", (int)channel_pitch->radio_min, (int)channel_pitch->radio_max);
@ -392,7 +392,7 @@ void Copter::print_radio_values()
cliSerial->printf("CH8: %d | %d\n", (int)g.rc_8.radio_min, (int)g.rc_8.radio_max);
}
void Copter::print_switch(uint8_t p, uint8_t m, bool b)
void Sub::print_switch(uint8_t p, uint8_t m, bool b)
{
cliSerial->printf("Pos %d:\t",p);
print_flight_mode(cliSerial, m);
@ -403,7 +403,7 @@ void Copter::print_switch(uint8_t p, uint8_t m, bool b)
cliSerial->printf("OFF\n");
}
void Copter::print_accel_offsets_and_scaling(void)
void Sub::print_accel_offsets_and_scaling(void)
{
const Vector3f &accel_offsets = ins.get_accel_offsets();
const Vector3f &accel_scale = ins.get_accel_scale();
@ -416,7 +416,7 @@ void Copter::print_accel_offsets_and_scaling(void)
(double)accel_scale.z); // YAW
}
void Copter::print_gyro_offsets(void)
void Sub::print_gyro_offsets(void)
{
const Vector3f &gyro_offsets = ins.get_gyro_offsets();
cliSerial->printf("G_off: %4.2f, %4.2f, %4.2f\n",
@ -428,7 +428,7 @@ void Copter::print_gyro_offsets(void)
#endif // CLI_ENABLED
// report_compass - displays compass information. Also called by compassmot.pde
void Copter::report_compass()
void Sub::report_compass()
{
cliSerial->printf("Compass\n");
print_divider();
@ -475,7 +475,7 @@ void Copter::report_compass()
print_blanks(1);
}
void Copter::print_blanks(int16_t num)
void Sub::print_blanks(int16_t num)
{
while(num > 0) {
num--;
@ -483,7 +483,7 @@ void Copter::print_blanks(int16_t num)
}
}
void Copter::print_divider(void)
void Sub::print_divider(void)
{
for (int i = 0; i < 40; i++) {
cliSerial->print("-");
@ -491,7 +491,7 @@ void Copter::print_divider(void)
cliSerial->println();
}
void Copter::print_enabled(bool b)
void Sub::print_enabled(bool b)
{
if(b)
cliSerial->print("en");
@ -500,7 +500,7 @@ void Copter::print_enabled(bool b)
cliSerial->print("abled\n");
}
void Copter::report_version()
void Sub::report_version()
{
cliSerial->printf("FW Ver: %d\n",(int)g.k_format_version);
print_divider();

24
ArduSub/switches.cpp
View File

@ -1,6 +1,6 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#include "Copter.h"
#include "Sub.h"
#define CONTROL_SWITCH_DEBOUNCE_TIME_MS 200
@ -18,7 +18,7 @@ static union {
uint32_t value;
} aux_con;
void Copter::read_control_switch()
void Sub::read_control_switch()
{
uint32_t tnow_ms = millis();
@ -75,7 +75,7 @@ void Copter::read_control_switch()
}
// check_if_auxsw_mode_used - Check to see if any of the Aux Switches are set to a given mode.
bool Copter::check_if_auxsw_mode_used(uint8_t auxsw_mode_check)
bool Sub::check_if_auxsw_mode_used(uint8_t auxsw_mode_check)
{
bool ret = g.ch7_option == auxsw_mode_check || g.ch8_option == auxsw_mode_check || g.ch9_option == auxsw_mode_check
|| g.ch10_option == auxsw_mode_check || g.ch11_option == auxsw_mode_check || g.ch12_option == auxsw_mode_check;
@ -84,7 +84,7 @@ bool Copter::check_if_auxsw_mode_used(uint8_t auxsw_mode_check)
}
// check_duplicate_auxsw - Check to see if any Aux Switch Functions are duplicated
bool Copter::check_duplicate_auxsw(void)
bool Sub::check_duplicate_auxsw(void)
{
bool ret = ((g.ch7_option != AUXSW_DO_NOTHING) && (g.ch7_option == g.ch8_option ||
g.ch7_option == g.ch9_option || g.ch7_option == g.ch10_option ||
@ -105,14 +105,14 @@ bool Copter::check_duplicate_auxsw(void)
return ret;
}
void Copter::reset_control_switch()
void Sub::reset_control_switch()
{
control_switch_state.last_switch_position = control_switch_state.debounced_switch_position = -1;
read_control_switch();
}
// read_3pos_switch
uint8_t Copter::read_3pos_switch(int16_t radio_in)
uint8_t Sub::read_3pos_switch(int16_t radio_in)
{
if (radio_in < AUX_SWITCH_PWM_TRIGGER_LOW) return AUX_SWITCH_LOW; // switch is in low position
if (radio_in > AUX_SWITCH_PWM_TRIGGER_HIGH) return AUX_SWITCH_HIGH; // switch is in high position
@ -120,7 +120,7 @@ uint8_t Copter::read_3pos_switch(int16_t radio_in)
}
// read_aux_switches - checks aux switch positions and invokes configured actions
void Copter::read_aux_switches()
void Sub::read_aux_switches()
{
uint8_t switch_position;
@ -195,7 +195,7 @@ void Copter::read_aux_switches()
}
// init_aux_switches - invoke configured actions at start-up for aux function where it is safe to do so
void Copter::init_aux_switches()
void Sub::init_aux_switches()
{
// set the CH7 ~ CH12 flags
aux_con.CH7_flag = read_3pos_switch(g.rc_7.radio_in);
@ -223,7 +223,7 @@ void Copter::init_aux_switches()
}
// init_aux_switch_function - initialize aux functions
void Copter::init_aux_switch_function(int8_t ch_option, uint8_t ch_flag)
void Sub::init_aux_switch_function(int8_t ch_option, uint8_t ch_flag)
{
// init channel options
switch(ch_option) {
@ -251,7 +251,7 @@ void Copter::init_aux_switch_function(int8_t ch_option, uint8_t ch_flag)
}
// do_aux_switch_function - implement the function invoked by the ch7 or ch8 switch
void Copter::do_aux_switch_function(int8_t ch_function, uint8_t ch_flag)
void Sub::do_aux_switch_function(int8_t ch_function, uint8_t ch_flag)
{
switch(ch_function) {
@ -585,7 +585,7 @@ void Copter::do_aux_switch_function(int8_t ch_function, uint8_t ch_flag)
}
// save_trim - adds roll and pitch trims from the radio to ahrs
void Copter::save_trim()
void Sub::save_trim()
{
// save roll and pitch trim
float roll_trim = ToRad((float)channel_roll->control_in/100.0f);
@ -597,7 +597,7 @@ void Copter::save_trim()
// auto_trim - slightly adjusts the ahrs.roll_trim and ahrs.pitch_trim towards the current stick positions
// meant to be called continuously while the pilot attempts to keep the copter level
void Copter::auto_trim()
void Sub::auto_trim()
{
if(auto_trim_counter > 0) {
auto_trim_counter--;

38
ArduSub/system.cpp
View File

@ -1,6 +1,6 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#include "Copter.h"
#include "Sub.h"
/*****************************************************************************
* The init_ardupilot function processes everything we need for an in - air restart
@ -12,7 +12,7 @@
#if CLI_ENABLED == ENABLED
// This is the help function
int8_t Copter::main_menu_help(uint8_t argc, const Menu::arg *argv)
int8_t Sub::main_menu_help(uint8_t argc, const Menu::arg *argv)
{
cliSerial->printf("Commands:\n"
" logs\n"
@ -37,14 +37,14 @@ const struct Menu::command main_menu_commands[] = {
// Create the top-level menu object.
MENU(main_menu, THISFIRMWARE, main_menu_commands);
int8_t Copter::reboot_board(uint8_t argc, const Menu::arg *argv)
int8_t Sub::reboot_board(uint8_t argc, const Menu::arg *argv)
{
hal.scheduler->reboot(false);
return 0;
}
// the user wants the CLI. It never exits
void Copter::run_cli(AP_HAL::UARTDriver *port)
void Sub::run_cli(AP_HAL::UARTDriver *port)
{
cliSerial = port;
Menu::set_port(port);
@ -71,16 +71,16 @@ void Copter::run_cli(AP_HAL::UARTDriver *port)
static void mavlink_delay_cb_static()
{
copter.mavlink_delay_cb();
sub.mavlink_delay_cb();
}
static void failsafe_check_static()
{
copter.failsafe_check();
sub.failsafe_check();
}
void Copter::init_ardupilot()
void Sub::init_ardupilot()
{
if (!hal.gpio->usb_connected()) {
// USB is not connected, this means UART0 may be a Xbee, with
@ -291,7 +291,7 @@ void Copter::init_ardupilot()
//******************************************************************************
//This function does all the calibrations, etc. that we need during a ground start
//******************************************************************************
void Copter::startup_INS_ground()
void Sub::startup_INS_ground()
{
// initialise ahrs (may push imu calibration into the mpu6000 if using that device).
ahrs.init();
@ -305,14 +305,14 @@ void Copter::startup_INS_ground()
}
// calibrate gyros - returns true if succesfully calibrated
bool Copter::calibrate_gyros()
bool Sub::calibrate_gyros()
{
// gyro offset calibration
copter.ins.init_gyro();
sub.ins.init_gyro();
// reset ahrs gyro bias
if (copter.ins.gyro_calibrated_ok_all()) {
copter.ahrs.reset_gyro_drift();
if (sub.ins.gyro_calibrated_ok_all()) {
sub.ahrs.reset_gyro_drift();
return true;
}
@ -320,7 +320,7 @@ bool Copter::calibrate_gyros()
}
// position_ok - returns true if the horizontal absolute position is ok and home position is set
bool Copter::position_ok()
bool Sub::position_ok()
{
// return false if ekf failsafe has triggered
if (failsafe.ekf) {
@ -332,7 +332,7 @@ bool Copter::position_ok()
}
// ekf_position_ok - returns true if the ekf claims it's horizontal absolute position estimate is ok and home position is set
bool Copter::ekf_position_ok()
bool Sub::ekf_position_ok()
{
if (!ahrs.have_inertial_nav()) {
// do not allow navigation with dcm position
@ -352,7 +352,7 @@ bool Copter::ekf_position_ok()
}
// optflow_position_ok - returns true if optical flow based position estimate is ok
bool Copter::optflow_position_ok()
bool Sub::optflow_position_ok()
{
#if OPTFLOW != ENABLED
return false;
@ -375,7 +375,7 @@ bool Copter::optflow_position_ok()
}
// update_auto_armed - update status of auto_armed flag
void Copter::update_auto_armed()
void Sub::update_auto_armed()
{
// disarm checks
if(ap.auto_armed){
@ -412,7 +412,7 @@ void Copter::update_auto_armed()
}
}
void Copter::check_usb_mux(void)
void Sub::check_usb_mux(void)
{
bool usb_check = hal.gpio->usb_connected();
if (usb_check == ap.usb_connected) {
@ -426,7 +426,7 @@ void Copter::check_usb_mux(void)
// frsky_telemetry_send - sends telemetry data using frsky telemetry
// should be called at 5Hz by scheduler
#if FRSKY_TELEM_ENABLED == ENABLED
void Copter::frsky_telemetry_send(void)
void Sub::frsky_telemetry_send(void)
{
frsky_telemetry.send_frames((uint8_t)control_mode);
}
@ -435,7 +435,7 @@ void Copter::frsky_telemetry_send(void)
/*
should we log a message type now?
*/
bool Copter::should_log(uint32_t mask)
bool Sub::should_log(uint32_t mask)
{
#if LOGGING_ENABLED == ENABLED
if (!(mask & g.log_bitmask) || in_mavlink_delay) {

12
ArduSub/takeoff.cpp
View File

@ -1,6 +1,6 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#include "Copter.h"
#include "Sub.h"
// This file contains the high-level takeoff logic for Loiter, PosHold, AltHold, Sport modes.
// The take-off can be initiated from a GCS NAV_TAKEOFF command which includes a takeoff altitude
@ -9,7 +9,7 @@
// return true if this flight mode supports user takeoff
// must_nagivate is true if mode must also control horizontal position
bool Copter::current_mode_has_user_takeoff(bool must_navigate)
bool Sub::current_mode_has_user_takeoff(bool must_navigate)
{
switch (control_mode) {
case GUIDED:
@ -25,7 +25,7 @@ bool Copter::current_mode_has_user_takeoff(bool must_navigate)
}
// initiate user takeoff - called when MAVLink TAKEOFF command is received
bool Copter::do_user_takeoff(float takeoff_alt_cm, bool must_navigate)
bool Sub::do_user_takeoff(float takeoff_alt_cm, bool must_navigate)
{
if (motors.armed() && ap.land_complete && current_mode_has_user_takeoff(must_navigate) && takeoff_alt_cm > current_loc.alt) {
@ -54,7 +54,7 @@ bool Copter::do_user_takeoff(float takeoff_alt_cm, bool must_navigate)
}
// start takeoff to specified altitude above home in centimeters
void Copter::takeoff_timer_start(float alt_cm)
void Sub::takeoff_timer_start(float alt_cm)
{
// calculate climb rate
float speed = MIN(wp_nav.get_speed_up(), MAX(g.pilot_velocity_z_max*2.0f/3.0f, g.pilot_velocity_z_max-50.0f));
@ -72,7 +72,7 @@ void Copter::takeoff_timer_start(float alt_cm)
}
// stop takeoff
void Copter::takeoff_stop()
void Sub::takeoff_stop()
{
takeoff_state.running = false;
takeoff_state.start_ms = 0;
@ -82,7 +82,7 @@ void Copter::takeoff_stop()
// pilot_climb_rate is both an input and an output
// takeoff_climb_rate is only an output
// has side-effect of turning takeoff off when timeout as expired
void Copter::takeoff_get_climb_rates(float& pilot_climb_rate, float& takeoff_climb_rate)
void Sub::takeoff_get_climb_rates(float& pilot_climb_rate, float& takeoff_climb_rate)
{
// return pilot_climb_rate if take-off inactive
if (!takeoff_state.running) {

20
ArduSub/test.cpp
View File

@ -1,6 +1,6 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#include "Copter.h"
#include "Sub.h"
#if CLI_ENABLED == ENABLED
@ -27,14 +27,14 @@ static const struct Menu::command test_menu_commands[] = {
// A Macro to create the Menu
MENU(test_menu, "test", test_menu_commands);
int8_t Copter::test_mode(uint8_t argc, const Menu::arg *argv)
int8_t Sub::test_mode(uint8_t argc, const Menu::arg *argv)
{
test_menu.run();
return 0;
}
#if HIL_MODE == HIL_MODE_DISABLED
int8_t Copter::test_baro(uint8_t argc, const Menu::arg *argv)
int8_t Sub::test_baro(uint8_t argc, const Menu::arg *argv)
{
print_hit_enter();
init_barometer(true);
@ -59,7 +59,7 @@ int8_t Copter::test_baro(uint8_t argc, const Menu::arg *argv)
}
#endif
int8_t Copter::test_compass(uint8_t argc, const Menu::arg *argv)
int8_t Sub::test_compass(uint8_t argc, const Menu::arg *argv)
{
uint8_t delta_ms_fast_loop;
uint8_t medium_loopCounter = 0;
@ -144,7 +144,7 @@ int8_t Copter::test_compass(uint8_t argc, const Menu::arg *argv)
return (0);
}
int8_t Copter::test_ins(uint8_t argc, const Menu::arg *argv)
int8_t Sub::test_ins(uint8_t argc, const Menu::arg *argv)
{
Vector3f gyro, accel;
print_hit_enter();
@ -176,7 +176,7 @@ int8_t Copter::test_ins(uint8_t argc, const Menu::arg *argv)
}
}
int8_t Copter::test_optflow(uint8_t argc, const Menu::arg *argv)
int8_t Sub::test_optflow(uint8_t argc, const Menu::arg *argv)
{
#if OPTFLOW == ENABLED
if(optflow.enabled()) {
@ -206,7 +206,7 @@ int8_t Copter::test_optflow(uint8_t argc, const Menu::arg *argv)
#endif // OPTFLOW == ENABLED
}
int8_t Copter::test_relay(uint8_t argc, const Menu::arg *argv)
int8_t Sub::test_relay(uint8_t argc, const Menu::arg *argv)
{
print_hit_enter();
delay(1000);
@ -232,7 +232,7 @@ int8_t Copter::test_relay(uint8_t argc, const Menu::arg *argv)
/*
* run a debug shell
*/
int8_t Copter::test_shell(uint8_t argc, const Menu::arg *argv)
int8_t Sub::test_shell(uint8_t argc, const Menu::arg *argv)
{
hal.util->run_debug_shell(cliSerial);
return 0;
@ -243,7 +243,7 @@ int8_t Copter::test_shell(uint8_t argc, const Menu::arg *argv)
/*
* test the rangefinders
*/
int8_t Copter::test_sonar(uint8_t argc, const Menu::arg *argv)
int8_t Sub::test_sonar(uint8_t argc, const Menu::arg *argv)
{
#if CONFIG_SONAR == ENABLED
sonar.init();
@ -268,7 +268,7 @@ int8_t Copter::test_sonar(uint8_t argc, const Menu::arg *argv)
}
#endif
void Copter::print_hit_enter()
void Sub::print_hit_enter()
{
cliSerial->printf("Hit Enter to exit.\n\n");
}

4
ArduSub/tuning.cpp
View File

@ -1,6 +1,6 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#include "Copter.h"
#include "Sub.h"
/*
* tuning.pde - function to update various parameters in flight using the ch6 tuning knob
@ -9,7 +9,7 @@
// tuning - updates parameters based on the ch6 tuning knob's position
// should be called at 3.3hz
void Copter::tuning() {
void Sub::tuning() {
// exit immediately if not tuning of when radio failsafe is invoked so tuning values are not set to zero
if ((g.radio_tuning <= 0) || failsafe.radio || failsafe.radio_counter != 0) {