Browse Source

Tracker: Adding alt_source enum

master
stefanlynka 9 years ago committed by Randy Mackay
parent
commit
6f88da07aa
  1. 2
      AntennaTracker/GCS_Mavlink.cpp
  2. 6
      AntennaTracker/defines.h
  3. 2
      AntennaTracker/tracking.cpp

2
AntennaTracker/GCS_Mavlink.cpp

@ -129,7 +129,7 @@ void Tracker::send_waypoint_request(mavlink_channel_t chan) @@ -129,7 +129,7 @@ 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;
float alt_diff = (g.alt_source == ALT_SOURCE_BARO) ? nav_status.alt_difference_baro : nav_status.alt_difference_gps;
mavlink_msg_nav_controller_output_send(
chan,

6
AntennaTracker/defines.h

@ -24,6 +24,12 @@ enum ServoType { @@ -24,6 +24,12 @@ enum ServoType {
SERVO_TYPE_CR=2
};
enum AltSource{
ALT_SOURCE_BARO=0,
ALT_SOURCE_GPS=1
};
// Logging parameters
#define MASK_LOG_ATTITUDE (1<<0)
#define MASK_LOG_GPS (1<<1)

2
AntennaTracker/tracking.cpp

@ -66,7 +66,7 @@ void Tracker::update_bearing_and_distance() @@ -66,7 +66,7 @@ void Tracker::update_bearing_and_distance()
// 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) {
if (g.alt_source == ALT_SOURCE_BARO) {
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));

Loading…
Cancel
Save