Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-05 15:33:57 -04:00
Code Issues Packages Projects Releases Wiki Activity

Sub: Refactor methods in navigation.cpp

Browse Source
This commit is contained in:
Jacob Walser 2017-04-06 23:21:37 -04:00
parent b1906d6c04
commit 3e471e808a
9 changed files with 16 additions and 106 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
ArduSub/ArduSub.cpp
View File

@ -33,7 +33,6 @@ const AP_Scheduler::Task Sub::scheduler_tasks[] = {
SCHED_TASK(update_batt_compass, 10, 120),
SCHED_TASK(read_rangefinder, 20, 100),
SCHED_TASK(update_altitude, 10, 100),
SCHED_TASK(run_nav_updates, 50, 100),
SCHED_TASK(three_hz_loop, 3, 75),
SCHED_TASK(update_turn_counter, 10, 50),
SCHED_TASK(compass_accumulate, 100, 100),

4
ArduSub/GCS_Mavlink.cpp
View File

@ -277,8 +277,8 @@ void NOINLINE Sub::send_nav_controller_output(mavlink_channel_t chan)
targets.x / 1.0e2f,
targets.y / 1.0e2f,
targets.z / 1.0e2f,
wp_bearing / 1.0e2f,
wp_distance / 1.0e2f,
wp_nav.get_wp_bearing_to_destination() / 1.0e2f,
wp_nav.get_wp_distance_to_destination() / 1.0e2f,
pos_control.get_alt_error() / 1.0e2f,
0,
0);

4
ArduSub/Sub.cpp
View File

@ -29,10 +29,6 @@ Sub::Sub(void) :
control_mode(MANUAL),
motors(MAIN_LOOP_RATE),
scaleLongDown(1),
wp_bearing(0),
home_bearing(0),
home_distance(0),
wp_distance(0),
auto_mode(Auto_WP),
guided_mode(Guided_WP),
circle_pilot_yaw_override(false),

16
ArduSub/Sub.h
View File

@ -298,15 +298,6 @@ private:
// Sometimes we need to remove the scaling for distance calcs
float scaleLongDown;
// Location & Navigation
int32_t wp_bearing;
// 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;
// distance between plane and next waypoint in cm.
uint32_t wp_distance;
// Auto
AutoMode auto_mode; // controls which auto controller is run
@ -650,13 +641,6 @@ private:
bool init_arm_motors(bool arming_from_gcs);
void init_disarm_motors();
void motors_output();
void run_nav_updates(void);
void calc_position();
void calc_distance_and_bearing();
void calc_wp_distance();
void calc_wp_bearing();
void calc_home_distance_and_bearing();
void run_autopilot();
void perf_info_reset();
void perf_ignore_this_loop();
void perf_info_check_loop_time(uint32_t time_in_micros);

13
ArduSub/commands_logic.cpp
View File

@ -149,8 +149,8 @@ bool Sub::start_command(const AP_Mission::Mission_Command& cmd)
// Verify command Handlers
/********************************************************************************/
// 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
// check to see if current command goal has been acheived
// called by mission library in mission.update()
bool Sub::verify_command_callback(const AP_Mission::Mission_Command& cmd)
{
if (control_mode == AUTO) {
@ -167,13 +167,10 @@ bool Sub::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
// check if current mission command has completed
bool Sub::verify_command(const AP_Mission::Mission_Command& cmd)
{
switch (cmd.id) {
//
// navigation commands
//
@ -760,9 +757,7 @@ bool Sub::verify_wait_delay()
bool Sub::verify_within_distance()
{
// update distance calculation
calc_wp_distance();
if (wp_distance < (uint32_t)MAX(condition_value,0)) {
if (wp_nav.get_wp_distance_to_destination() < (uint32_t)MAX(condition_value,0)) {
condition_value = 0;
return true;
}

8
ArduSub/control_auto.cpp
View File

@ -34,11 +34,13 @@ bool Sub::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
// auto_run - runs the appropriate auto controller
// according to the current auto_mode
// should be called at 100hz or more
void Sub::auto_run()
{
mission.update();
// call the correct auto controller
switch (auto_mode) {

4
ArduSub/fence.cpp
View File

@ -11,6 +11,10 @@ void Sub::fence_check()
uint8_t new_breaches; // the type of fence that has been breached
uint8_t orig_breaches = fence.get_breaches();
Vector3f home = pv_location_to_vector(ahrs.get_home());
Vector3f curr = inertial_nav.get_position();
int32_t home_distance = pv_get_horizontal_distance_cm(curr, home);
// give fence library our current distance from home in meters
fence.set_home_distance(home_distance*0.01f);

3
ArduSub/motors.cpp
View File

@ -53,8 +53,7 @@ bool Sub::init_arm_motors(bool arming_from_gcs)
// Reset home position if it has already been set before (but not locked)
set_home_to_current_location();
}
calc_distance_and_bearing();
// enable gps velocity based centrefugal force compensation
ahrs.set_correct_centrifugal(true);
hal.util->set_soft_armed(true);

69
ArduSub/navigation.cpp
View File

@ -1,69 +0,0 @@
#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 Sub::run_nav_updates(void)
{
// calculate distance and bearing for reporting and autopilot decisions
calc_distance_and_bearing();
// run autopilot to make high level decisions about control modes
run_autopilot();
}
// calc_distance_and_bearing - calculate distance and bearing to next waypoint and home
void Sub::calc_distance_and_bearing()
{
calc_wp_distance();
calc_wp_bearing();
calc_home_distance_and_bearing();
}
// calc_wp_distance - calculate distance to next waypoint for reporting and autopilot decisions
void Sub::calc_wp_distance()
{
// get target from loiter or wpinav controller
if (control_mode == CIRCLE) {
wp_distance = wp_nav.get_loiter_distance_to_target();
} else if (control_mode == AUTO || (control_mode == GUIDED && guided_mode == Guided_WP)) {
wp_distance = wp_nav.get_wp_distance_to_destination();
} else {
wp_distance = 0;
}
}
// calc_wp_bearing - calculate bearing to next waypoint for reporting and autopilot decisions
void Sub::calc_wp_bearing()
{
// get target from loiter or wpinav controller
if (control_mode == CIRCLE) {
wp_bearing = wp_nav.get_loiter_bearing_to_target();
} else if (control_mode == AUTO || (control_mode == GUIDED && guided_mode == Guided_WP)) {
wp_bearing = wp_nav.get_wp_bearing_to_destination();
} else {
wp_bearing = 0;
}
}
// calc_home_distance_and_bearing - calculate distance and bearing to home for reporting and autopilot decisions
void Sub::calc_home_distance_and_bearing()
{
// calculate home distance and bearing
if (position_ok()) {
Vector3f home = pv_location_to_vector(ahrs.get_home());
Vector3f curr = inertial_nav.get_position();
home_distance = pv_get_horizontal_distance_cm(curr, home);
home_bearing = pv_get_bearing_cd(curr,home);
}
}
// run_autopilot - highest level call to process mission commands
void Sub::run_autopilot()
{
if (control_mode == AUTO) {
// update state of mission
// may call commands_process.pde's start_command and verify_command functions
mission.update();
}
}