Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

Tracker: Allows choice of altitude source

Browse Source
This commit is contained in:
stefanlynka 2016-05-24 20:53:57 +09:00 committed by Randy Mackay
parent fd61a903f9
commit 1293b16589
5 changed files with 23 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
AntennaTracker/GCS_Mavlink.cpp
View File

@ -127,6 +127,8 @@ void Tracker::send_waypoint_request(mavlink_channel_t chan)
void Tracker::send_nav_controller_output(mavlink_channel_t chan)
{
float alt_diff = (g.alt_source == 0) ? nav_status.alt_difference_baro : nav_status.alt_difference_gps;
mavlink_msg_nav_controller_output_send(
chan,
0,
@ -134,7 +136,7 @@ void Tracker::send_nav_controller_output(mavlink_channel_t chan)
nav_status.bearing,
nav_status.bearing,
nav_status.distance,
nav_status.alt_difference_baro,
alt_diff,
0,
0);
}

7
AntennaTracker/Parameters.cpp
View File

@ -214,6 +214,13 @@ const AP_Param::Info Tracker::var_info[] = {
// @User: Standard
GSCALAR(distance_min, "DISTANCE_MIN", DISTANCE_MIN_DEFAULT),
// @Param: ALT_SOURCE
// @DisplayName: Altitude Source
// @Description: What provides altitude information for vehicle
// @Values: 0:Barometer,1:GPS
// @User: Standard
GSCALAR(alt_source, "ALT_SOURCE", 0),
// barometer ground calibration. The GND_ prefix is chosen for
// compatibility with previous releases of ArduPlane
// @Group: GND_

2
AntennaTracker/Parameters.h
View File

@ -99,6 +99,7 @@ public:
//
k_param_serial_manager, // serial manager library
k_param_servo_yaw_type,
k_param_alt_source,
//
// 200 : Radio settings
@ -136,6 +137,7 @@ public:
AP_Float startup_delay;
AP_Int8 servo_pitch_type;
AP_Int8 servo_yaw_type;
AP_Int8 alt_source;
AP_Float onoff_yaw_rate;
AP_Float onoff_pitch_rate;
AP_Float onoff_yaw_mintime;

5
AntennaTracker/Tracker.h
View File

@ -159,14 +159,15 @@ private:
float bearing; // bearing to vehicle in centi-degrees
float distance; // distance to vehicle in meters
float pitch; // pitch to vehicle in degrees (positive means vehicle is above tracker, negative means below)
float alt_difference_baro; // altitude difference between tracker and vehicle in meters. positive value means vehicle is above tracker
float alt_difference_baro; // altitude difference between tracker and vehicle in meters according to the barometer. positive value means vehicle is above tracker
float alt_difference_gps; // altitude difference between tracker and vehicle in meters according to the gps. positive value means vehicle is above tracker
float altitude_offset; // offset in meters which is added to tracker altitude to align altitude measurements with vehicle's barometer
bool manual_control_yaw : 1;// true if tracker yaw is under manual control
bool manual_control_pitch : 1;// true if tracker pitch is manually controlled
bool need_altitude_calibration : 1;// true if tracker altitude has not been determined (true after startup)
bool scan_reverse_pitch : 1;// controls direction of pitch movement in SCAN mode
bool scan_reverse_yaw : 1;// controls direction of yaw movement in SCAN mode
} nav_status = {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, false, false, true, false, false};
} nav_status = {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, false, false, true, false, false};
// Servo state
struct {

8
AntennaTracker/tracking.cpp
View File

@ -57,10 +57,18 @@ void Tracker::update_bearing_and_distance()
// calculate distance to vehicle
nav_status.distance = get_distance(current_loc, vehicle.location_estimate);
// calculate altitude difference to vehicle using gps
nav_status.alt_difference_gps = vehicle.location.alt - current_loc.alt;
// calculate pitch to vehicle
// To-Do: remove need for check of control_mode
if (control_mode != SCAN && !nav_status.manual_control_pitch) {
if(g.alt_source == 0){
nav_status.pitch = degrees(atan2f(nav_status.alt_difference_baro, nav_status.distance));
}
else{
nav_status.pitch = degrees(atan2f(nav_status.alt_difference_gps, nav_status.distance));
}
}
}