Tools/AntennaTracker: completed intial tracker code

Now works with 2 servo alt-azimuth mounted antenna tracking mount. Tested
on Flymaple, with Eagle Tree antenna tracker.

Tools/AntennaTracker/GCS_Mavlink.pde

@@ -28,9 +28,9 @@ static NOINLINE void send_heartbeat(mavlink_channel_t chan)
         chan,
         MAV_TYPE_ANTENNA_TRACKER,
         MAV_AUTOPILOT_ARDUPILOTMEGA,
+        MAV_MODE_FLAG_AUTO_ENABLED,
         0,
-        0,
+        MAV_STATE_ACTIVE);
-        0);
 }
 
 static NOINLINE void send_attitude(mavlink_channel_t chan)
@@ -56,16 +56,12 @@ static void NOINLINE send_location(mavlink_channel_t chan)
     } else {
         fix_time = hal.scheduler->millis();
     }
-
-    Location loc;
-    ahrs.get_position(loc);
-
     mavlink_msg_global_position_int_send(
         chan,
         fix_time,
-        loc.lat,                // in 1E7 degrees
+        current_loc.lat,                // in 1E7 degrees
-        loc.lng,                // in 1E7 degrees
+        current_loc.lng,                // in 1E7 degrees
-        g_gps->altitude_cm * 10,        // millimeters above sea level
+        current_loc.alt * 10,        // millimeters above sea level
         0,
         g_gps->velocity_north() * 100,  // X speed cm/s (+ve North)
         g_gps->velocity_east()  * 100,  // Y speed cm/s (+ve East)
@@ -75,6 +71,11 @@ static void NOINLINE send_location(mavlink_channel_t chan)
 
 static void NOINLINE send_gps_raw(mavlink_channel_t chan)
 {
+    static uint32_t last_send_time;
+    if (last_send_time != 0 && last_send_time == g_gps->last_fix_time && g_gps->status() >= GPS::GPS_OK_FIX_3D) {
+        return;
+    }
+    last_send_time = g_gps->last_fix_time;
     mavlink_msg_gps_raw_int_send(
         chan,
         g_gps->last_fix_time*(uint64_t)1000,
@@ -209,6 +210,14 @@ static void NOINLINE send_hwstatus(mavlink_channel_t chan)
         hal.i2c->lockup_count());
 }
 
+static void NOINLINE send_waypoint_request(mavlink_channel_t chan)
+{
+    if (chan == MAVLINK_COMM_0)
+        gcs0.queued_waypoint_send();
+    else
+        gcs3.queued_waypoint_send();
+}
+
 static void NOINLINE send_statustext(mavlink_channel_t chan)
 {
     mavlink_statustext_t *s = (chan == MAVLINK_COMM_0?&gcs0.pending_status:&gcs3.pending_status);
@@ -292,6 +301,11 @@ static bool mavlink_try_send_message(mavlink_channel_t chan, enum ap_message id,
         }
         break;
 
+    case MSG_NEXT_WAYPOINT:
+        CHECK_PAYLOAD_SIZE(MISSION_REQUEST);
+        send_waypoint_request(chan);
+        break;
+
     case MSG_STATUSTEXT:
         CHECK_PAYLOAD_SIZE(STATUSTEXT);
         send_statustext(chan);
@@ -315,7 +329,6 @@ static bool mavlink_try_send_message(mavlink_channel_t chan, enum ap_message id,
     case MSG_VFR_HUD:
     case MSG_RADIO_IN:
     case MSG_SYSTEM_TIME:
-    case MSG_NEXT_WAYPOINT:
     case MSG_LIMITS_STATUS:
     case MSG_FENCE_STATUS:
     case MSG_SIMSTATE:
@@ -514,6 +527,7 @@ GCS_MAVLINK::update(void)
 
     // Update packet drops counter
     packet_drops += status.packet_rx_drop_count;
+
 }
 
 // see if we should send a stream now. Called at 50Hz
@@ -624,6 +638,8 @@ GCS_MAVLINK::send_text_P(gcs_severity severity, const prog_char_t *str)
 
 void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
 {
+//    hal.uartA->printf("handleMessage %d\n", msg->msgid);
+
     switch (msg->msgid) {
 
     case MAVLINK_MSG_ID_REQUEST_DATA_STREAM:
@@ -811,7 +827,182 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
         break;
     }
 
-    case MAVLINK_MSG_ID_GLOBAL_POSITION_INT: {
+    case MAVLINK_MSG_ID_COMMAND_LONG:
+    {
+        // decode
+        mavlink_command_long_t packet;
+        mavlink_msg_command_long_decode(msg, &packet);
+        if (mavlink_check_target(packet.target_system, packet.target_component)) break;
+        
+        uint8_t result = MAV_RESULT_UNSUPPORTED;
+        
+        // do command
+        send_text_P(SEVERITY_LOW,PSTR("command received: "));
+        
+        switch(packet.command) {
+            
+            case MAV_CMD_PREFLIGHT_CALIBRATION:
+            {
+                if (packet.param1 == 1 ||
+                    packet.param2 == 1) {
+                    calibrate_ins();
+                } else if (packet.param3 == 1) {
+                    init_barometer();
+                }
+                if (packet.param4 == 1) {
+                    // Cant trim radio
+                }
+#if !defined( __AVR_ATmega1280__ )
+                if (packet.param5 == 1) {
+                    float trim_roll, trim_pitch;
+                    AP_InertialSensor_UserInteract_MAVLink interact(chan);
+                    if(ins.calibrate_accel(&interact, trim_roll, trim_pitch)) {
+                        // reset ahrs's trim to suggested values from calibration routine
+                        ahrs.set_trim(Vector3f(trim_roll, trim_pitch, 0));
+                    }
+                }
+#endif
+                result = MAV_RESULT_ACCEPTED;
+                break;
+            }
+
+            case MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN:
+            {
+                if (packet.param1 == 1 || packet.param1 == 3) {
+                    // when packet.param1 == 3 we reboot to hold in bootloader
+                    hal.scheduler->reboot(packet.param1 == 3);
+                    result = MAV_RESULT_ACCEPTED;
+                }
+                break;
+            }
+
+            default:
+                break;
+        }
+        mavlink_msg_command_ack_send(
+            chan,
+            packet.command,
+            result);
+        
+        break;
+    }
+         
+    // When mavproxy 'wp sethome' 
+    case MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST:
+    {
+        // decode
+        mavlink_mission_write_partial_list_t packet;
+        mavlink_msg_mission_write_partial_list_decode(msg, &packet);
+        if (mavlink_check_target(packet.target_system,packet.target_component)) break;
+
+        if (packet.start_index == 0)
+        {
+            // New home at wp index 0. Ask for it
+            waypoint_receiving = true;
+            waypoint_request_i = 0;
+            waypoint_request_last = 0;
+            send_message(MSG_NEXT_WAYPOINT);
+            waypoint_receiving = true;
+        }
+        break;
+    }
+
+    // XXX receive a WP from GCS and store in EEPROM if it is HOME
+    case MAVLINK_MSG_ID_MISSION_ITEM:
+    {
+        // decode
+        mavlink_mission_item_t packet;
+        uint8_t result = MAV_MISSION_ACCEPTED;
+
+        mavlink_msg_mission_item_decode(msg, &packet);
+        if (mavlink_check_target(packet.target_system,packet.target_component)) break;
+
+        struct Location tell_command = {};
+
+        // defaults
+        tell_command.id = packet.command;
+
+        switch (packet.frame)
+        {
+        case MAV_FRAME_MISSION:
+        case MAV_FRAME_GLOBAL:
+        {
+            tell_command.lat = 1.0e7f*packet.x;                                     // in as DD converted to * t7
+            tell_command.lng = 1.0e7f*packet.y;                                     // in as DD converted to * t7
+            tell_command.alt = packet.z*1.0e2f;                                     // in as m converted to cm
+            tell_command.options = 0;                                     // absolute altitude
+            break;
+        }
+
+#ifdef MAV_FRAME_LOCAL_NED
+        case MAV_FRAME_LOCAL_NED:                         // local (relative to home position)
+        {
+            tell_command.lat = 1.0e7f*ToDeg(packet.x/
+                                           (RADIUS_OF_EARTH*cosf(ToRad(home.lat/1.0e7f)))) + home.lat;
+            tell_command.lng = 1.0e7f*ToDeg(packet.y/RADIUS_OF_EARTH) + home.lng;
+            tell_command.alt = -packet.z*1.0e2f;
+            tell_command.options = MASK_OPTIONS_RELATIVE_ALT;
+            break;
+        }
+#endif
+
+#ifdef MAV_FRAME_LOCAL
+        case MAV_FRAME_LOCAL:                         // local (relative to home position)
+        {
+            tell_command.lat = 1.0e7f*ToDeg(packet.x/
+                                           (RADIUS_OF_EARTH*cosf(ToRad(home.lat/1.0e7f)))) + home.lat;
+            tell_command.lng = 1.0e7f*ToDeg(packet.y/RADIUS_OF_EARTH) + home.lng;
+            tell_command.alt = packet.z*1.0e2f;
+            tell_command.options = MASK_OPTIONS_RELATIVE_ALT;
+            break;
+        }
+#endif
+
+        case MAV_FRAME_GLOBAL_RELATIVE_ALT:                         // absolute lat/lng, relative altitude
+        {
+            tell_command.lat = 1.0e7f * packet.x;                                     // in as DD converted to * t7
+            tell_command.lng = 1.0e7f * packet.y;                                     // in as DD converted to * t7
+            tell_command.alt = packet.z * 1.0e2f;
+            tell_command.options = MASK_OPTIONS_RELATIVE_ALT;                                     // store altitude relative!! Always!!
+            break;
+        }
+
+        default:
+            result = MAV_MISSION_UNSUPPORTED_FRAME;
+            break;
+        }
+
+        if (result != MAV_MISSION_ACCEPTED) goto mission_failed;
+
+        // Check if receiving waypoints (mission upload expected)
+        if (!waypoint_receiving) {
+            result = MAV_MISSION_ERROR;
+            goto mission_failed;
+        }
+
+        // check if this is the HOME wp
+        if (packet.seq == 0) {
+            set_home(tell_command); // New home in EEPROM
+            send_text_P(SEVERITY_LOW,PSTR("new HOME received"));
+            waypoint_receiving = false;
+
+        }
+
+
+
+mission_failed:
+        // we are rejecting the mission/waypoint
+        mavlink_msg_mission_ack_send(
+            chan,
+            msg->sysid,
+            msg->compid,
+            result);
+        break;
+    }
+
+
+    case MAVLINK_MSG_ID_GLOBAL_POSITION_INT: 
+    {
         // decode
         mavlink_global_position_int_t packet;
         mavlink_msg_global_position_int_decode(msg, &packet);

Tools/AntennaTracker/Parameters.h

@@ -69,7 +69,12 @@ public:
         k_param_pidYaw2Srv,
 
         k_param_channel_yaw = 200,
-        k_param_channel_pitch
+        k_param_channel_pitch,
+
+        //
+        // 220: Waypoint data
+        //
+        k_param_command_total = 220,
 
         // 254,255: reserved
     };
@@ -86,6 +91,10 @@ public:
 
     AP_Int8 compass_enabled;
 
+    // Waypoints
+    //
+    AP_Int8 command_total; // 1 if HOME is set
+
     // PID controllers
     PID         pidPitch2Srv;
     PID         pidYaw2Srv;

Tools/AntennaTracker/Parameters.pde

@@ -100,6 +100,14 @@ const AP_Param::Info var_info[] PROGMEM = {
 	GGROUP(pidPitch2Srv,       "PITCH2SRV_", PID),
 	GGROUP(pidYaw2Srv,         "YAW2SRV_", PID),
 
+    // @Param: CMD_TOTAL
+    // @DisplayName: Number of loaded mission items
+    // @Description: Set to 1 if HOME location has been loaded by the ground station. Do not change this manually.
+    // @Range: 1 255
+    // @User: Advanced
+    GSCALAR(command_total,          "CMD_TOTAL",      0),
+
+
     AP_VAREND
 };
 

Tools/AntennaTracker/config.h

@@ -81,3 +81,13 @@
 #ifndef SERIAL2_BUFSIZE
  # define SERIAL2_BUFSIZE 256
 #endif
+
+//////////////////////////////////////////////////////////////////////////////
+// Developer Items
+//
+
+// use this to completely disable the CLI
+#ifndef CLI_ENABLED
+ # define CLI_ENABLED ENABLED
+#endif
+

Tools/AntennaTracker/defines.h

@@ -30,4 +30,25 @@
 #define AP_COMPASS_PX4       2
 #define AP_COMPASS_HIL       3
 
+// In AntennaTracker, EEPROM is used to store waypoint 0 = HOME location, if available
+// EEPROM addresses
+#define EEPROM_MAX_ADDR         4096
+// parameters get the first 1280 bytes of EEPROM, remainder is for waypoints
+#define WP_START_BYTE 0x500 // where in memory home WP is stored + all other
+                            // WP
+#define WP_SIZE 15
+
+// Commands - Note that APM now uses a subset of the MAVLink protocol commands.  See enum MAV_CMD in the GCS_Mavlink library
+#define CMD_BLANK 0 // there is no command stored in the mem location requested
+#define NO_COMMAND 0
+#define WAIT_COMMAND 255
+
+// Command/Waypoint/Location Options Bitmask
+//--------------------
+#define MASK_OPTIONS_RELATIVE_ALT       (1<<0)          // 1 = Relative
+                                                        // altitude
+#define MASK_OPTIONS_LOITER_DIRECTION   (1<<2)          // 0 = CW
+                                                        // 1 = CCW
+
 #endif // _DEFINES_H
+

Tools/AntennaTracker/sensors.pde

@@ -61,5 +61,7 @@ static void barometer_accumulate(void)
 static void update_GPS(void)
 {
     g_gps->update();
+    // REVISIT: add compass variation with compass.set_initial_location(g_gps->latitude, g_gps->longitude);
+    // when have a solid GPS fix. Also init_home();
 }
 

Tools/AntennaTracker/system.pde

@@ -63,8 +63,8 @@ static void init_tracker()
     hal.uartC->set_blocking_writes(false);
 
     // setup antenna control PWM channels
-    channel_yaw.set_angle(4500);
+    channel_yaw.set_angle(18000); // Yaw is expected to drive antenna azimuth -180-0-180
-    channel_pitch.set_angle(4500);
+    channel_pitch.set_angle(9000); // Pitch is expected to drive elevation -90-0-90
 
     channel_yaw.output_trim();
     channel_pitch.output_trim();
@@ -75,8 +75,24 @@ static void init_tracker()
     channel_yaw.enable_out();
     channel_pitch.enable_out();
 
+    home_loc = get_home_eeprom(); // GPS may update this later
+
     gcs_send_text_P(SEVERITY_LOW,PSTR("\nReady to track."));
-    hal.scheduler->delay(1000);
+    hal.scheduler->delay(1000); // Why????
+}
+
+// Level the tracker by calibrating the INS
+// Requires that the tracker be physically 'level' and horizontal
+static void calibrate_ins()
+{
+    gcs_send_text_P(SEVERITY_MEDIUM, PSTR("Beginning INS calibration; do not move tracker"));
+    ahrs.init();
+    ahrs.set_fly_forward(true);
+    ins.init(AP_InertialSensor::COLD_START, ins_sample_rate);
+    ins.init_accel();
+    ahrs.set_trim(Vector3f(0, 0, 0));
+    ahrs.reset();
+    init_barometer();
 }
 
 // updates the status of the notify objects
@@ -106,4 +122,72 @@ static uint32_t map_baudrate(int8_t rate, uint32_t default_baud)
     return default_baud;
 }
 
+/*
+  fetch HOME from EEPROM
+*/
+static struct Location get_home_eeprom()
+{
+    struct Location temp;
+    uint16_t mem;
 
+    // Find out proper location in memory by using the start_byte position + the index
+    // --------------------------------------------------------------------------------
+    if (g.command_total.get() == 0) {
+        memset(&temp, 0, sizeof(temp));
+        temp.id = CMD_BLANK;
+    }else{
+        // read WP position
+        mem = WP_START_BYTE;
+        temp.id = hal.storage->read_byte(mem);
+
+        mem++;
+        temp.options = hal.storage->read_byte(mem);
+
+        mem++;
+        temp.p1 = hal.storage->read_byte(mem);
+
+        mem++;
+        temp.alt = hal.storage->read_dword(mem);
+
+        mem += 4;
+        temp.lat = hal.storage->read_dword(mem);
+
+        mem += 4;
+        temp.lng = hal.storage->read_dword(mem);
+    }
+
+    return temp;
+}
+
+static void set_home_eeprom(struct Location temp)
+{
+    uint16_t mem = WP_START_BYTE;
+
+    hal.storage->write_byte(mem, temp.id);
+
+    mem++;
+    hal.storage->write_byte(mem, temp.options);
+
+    mem++;
+    hal.storage->write_byte(mem, temp.p1);
+
+    mem++;
+    hal.storage->write_dword(mem, temp.alt);
+
+    mem += 4;
+    hal.storage->write_dword(mem, temp.lat);
+
+    mem += 4;
+    hal.storage->write_dword(mem, temp.lng);
+
+    // Now have a home location in EEPROM
+    g.command_total.set_and_save(1); // At most 1 entry for HOME
+}
+
+static void set_home(struct Location temp)
+{
+    if (g.compass_enabled)
+        compass.set_initial_location(temp.lat, temp.lng);
+    set_home_eeprom(temp);
+    home_loc = temp;
+}

Tools/AntennaTracker/tracking.pde

@@ -4,31 +4,149 @@
    state of the vehicle we are tracking
  */
 static struct {
-    Location location;
+    Location location; // lat, long in degrees * 10^7; alt in meters * 100
     uint32_t last_update_us;
     float heading; // degrees
     float ground_speed; // m/s
 } vehicle;
 
-static Location our_location;
-
 /**
-   update the pitch servo. The aim is to drive the boards pitch to the
+   update the pitch (elevation) servo. The aim is to drive the boards ahrs pitch to the
-   requested pitch
+   requested pitch, so the board (and therefore the antenna) will be pointing at the target
  */
 static void update_pitch_servo(float pitch)
 {
-    channel_pitch.servo_out = g.pidPitch2Srv.get_pid_4500(degrees(ahrs.pitch) - pitch);
+    // degrees(ahrs.pitch) is -90 to 90, where 0 is horizontal
+    // pitch argument is -90 to 90, where 0 is horizontal
+    // servo_out is in 100ths of a degree
+    float ahrs_pitch = degrees(ahrs.pitch);
+    int32_t err = (ahrs_pitch - pitch) * 100.0; 
+    // Need to configure your servo so that increasing servo_out causes increase in pitch/elevation (ie pointing higher into the sky,
+    // above the horizon. On my antenna tracker this requires the pitch/elevation servo to be reversed
+    // param set RC2_REV -1
+    //
+    // The pitch servo (RC channel 2) is configured for servo_out of -9000-0-9000 servo_out, 
+    // which will drive the servo from RC2_MIN to RC2_MAX usec pulse width.
+    // Therefore, you must set RC2_MIN and RC2_MAX so that your servo drives the antenna altitude between -90 to 90 exactly
+    // To drive my HS-645MG servos through their full 180 degrees of rotational range, I have to set:
+    // param set RC2_MAX 2540
+    // param set RC2_MIN 640
+    //
+    // You will also need to tune the pitch PID to suit your antenna and servos. I use:
+    // PITCH2SRV_P      0.100000
+    // PITCH2SRV_I      0.020000
+    // PITCH2SRV_D      0.000000
+    // PITCH2SRV_IMAX   4000.000000
+    int32_t new_servo_out = channel_pitch.servo_out - g.pidPitch2Srv.get_pid(err);
+    channel_pitch.servo_out = constrain_float(new_servo_out, -9000, 9000);
     channel_pitch.calc_pwm();
     channel_pitch.output();
 }
 
 /**
-   update the yaw servo
+   update the yaw (azimuth) servo. The aim is to drive the boards ahrs yaw to the
+   requested yaw, so the board (and therefore the antenna) will be pinting at the target
  */
 static void update_yaw_servo(float yaw)
 {
-    channel_yaw.servo_out = g.pidYaw2Srv.get_pid_4500(degrees(ahrs.yaw) - yaw);    
+    // degrees(ahrs.yaw) is -180 to 180, where 0 is north
+    float ahrs_yaw = degrees(ahrs.yaw);
+    // yaw argument is 0 to 360 where 0 and 360 are north
+    // Make yaw -180-0-180 too
+    if (yaw > 180)
+        yaw = yaw - 360;
+
+    // Antenna as Ballerina. Use with antenna that do not have continuously rotating servos, ie at some point in rotation
+    // the servo limits are reached and the servo has to slew 360 degrees to the 'other side' to keep tracking.
+    //
+    // This algorithm accounts for the fact that the antenna mount may not be aligned with North 
+    // (in fact, any alignment is permissable), and that the alignment may change (possibly rapidly) over time 
+    // (as when the antenna is mounted on a moving, turning vehicle)
+    // When the servo is being forced beyond its limits, it rapidly slews to the 'other side' then normal tracking takes over.
+    //
+    // Caution: this whole system is compromised by the fact that the ahrs_yaw reported by the compass system lags 
+    // the actual yaw by about 5 seconds, and also periodically realigns itself with about a 30 second period, 
+    // which makes it very hard to be completely sure exactly where the antenna is _really_ pointing
+    // especially during the high speed slew. This can cause odd pointing artifacts from time to time. The best strategy is to
+    // position and point the mount so the aircraft does not 'go behind' the antenna (if possible)
+    //
+    // With my antenna mount, large pwm output drives the antenna anticlockise, so need:
+    // param set RC1_REV -1
+    // to reverse the servo. Yours may be different
+    //
+    // You MUST set RC1_MIN and RC1_MAX so that your servo drives the antenna azimuth from -180 to 180 relative
+    // to the mount.
+    // To drive my HS-645MG servos through their full 180 degrees of rotational range and therefore the
+    // antenna through a full 360 degrees, I have to set:
+    // param set RC1_MAX 2380
+    // param set RC1_MIN 680
+    // According to the specs at https://www.servocity.com/html/hs-645mg_ultra_torque.html,
+    // that should be 600 through 2400, but the azimuth gearing in my antenna pointer is not exactly 2:1
+    int32_t err = (ahrs_yaw - yaw) * 100.0;
+    static int32_t last_err = 0.0;
+
+    // Correct for wrapping yaw at +-180
+    // so we get the error to the _closest_ version of the target azimuth
+    // +ve error requires anticlockwise motion (ie towards more negative yaw)
+    if (err > 18000)
+        err -= 36000;
+    else if (err < -18000)
+        err += 36000;
+
+    static int32_t slew_to = 0;
+
+    int32_t servo_err = channel_yaw.servo_out - err; // Servo position we need to get to
+    if (   !slew_to
+        && servo_err > 19000 // 10 degreee deadband
+        && err < 0
+        && last_err > err)
+    {
+        // We need to be beyond the servo limits and the error magnitude is increasing
+        // Slew most of the way to the other side at high speed...
+        slew_to = servo_err - 27000;
+    }
+    else if (   !slew_to
+             && servo_err < -19000 // 10 degreee deadband
+             && err > 0
+             && last_err < err)
+    {
+        // We need to be beyond the servo limits and the error magnitude is increasing
+        // Slew most of the way to the other side at high speed...
+        slew_to = servo_err + 27000;
+    }
+    else if (   slew_to < 0
+             && err > 0
+             && last_err < err)
+    {
+        // ...then let normal tracking take over
+        slew_to = 0;
+    }
+    else if (   slew_to > 0
+             && err < 0
+             && last_err > err)
+    {
+        // ...then let normal tracking take over
+        slew_to = 0;
+    }
+
+    if (slew_to)
+    {
+        channel_yaw.servo_out = slew_to;
+    }
+    else
+    {
+        // Normal tracking
+        // You will need to tune the yaw PID to suit your antenna and servos
+        // For my servos, suitable PID settings are:
+        // param set YAW2SRV_P 0.1
+        // param set YAW2SRV_I 0.05
+        // param set YAW2SRV_D 0
+        // param set YAW2SRV_IMAX 4000
+        
+        int32_t new_servo_out = channel_yaw.servo_out - g.pidYaw2Srv.get_pid(err);
+        channel_yaw.servo_out = constrain_float(new_servo_out, -18000, 18000);
+    }
+    last_err = err;
     channel_yaw.calc_pwm();
     channel_yaw.output();
 }
@@ -46,16 +164,17 @@ static void update_tracking(void)
 
     // update our position if we have at least a 2D fix
     if (g_gps->status() >= GPS::GPS_OK_FIX_2D) {
-        our_location.lat = g_gps->latitude;
+        current_loc.lat = g_gps->latitude;
-        our_location.lng = g_gps->longitude;
+        current_loc.lng = g_gps->longitude;
-        our_location.alt = 0; // assume ground level for now
+        current_loc.alt = 0; // assume ground level for now REVISIT: WHY?
+    }
+    else {
+        current_loc = home_loc; // dont know any better
     }
-
     // calculate the bearing to the vehicle
-    float bearing  = get_bearing_cd(our_location, vehicle.location) * 0.01f;
+    float bearing  = get_bearing_cd(current_loc, vehicle.location) * 0.01f;
-    float distance = get_distance(our_location, vehicle.location);
+    float distance = get_distance(current_loc, vehicle.location);
-    float pitch    = degrees(atan2(vehicle.location.alt - our_location.alt, distance));
+    float pitch    = degrees(atan2((vehicle.location.alt - current_loc.alt)/100, distance));
-    
     // update the servos
     update_pitch_servo(pitch);
     update_yaw_servo(bearing);
@@ -79,3 +198,4 @@ static void tracking_update_position(const mavlink_global_position_int_t &msg)
     vehicle.ground_speed = pythagorous2(msg.vx, msg.vy) * 0.01f;
     vehicle.last_update_us = hal.scheduler->micros();    
 }
+