AntennaTracker/tracking.cpp

   1 #include "Tracker.h"
   2
   3 /**
   4   update_vehicle_position_estimate - updates estimate of vehicle positions
   5   should be called at 50hz
   6  */
   7 void Tracker::update_vehicle_pos_estimate()
   8 {
   9     // calculate time since last actual position update
  10     float dt = (AP_HAL::micros() - vehicle.last_update_us) * 1.0e-6f;
  11
  12     // if less than 5 seconds since last position update estimate the position
  13     if (dt < TRACKING_TIMEOUT_SEC) {
  14         // project the vehicle position to take account of lost radio packets
  15         vehicle.location_estimate = vehicle.location;
  16         float north_offset = vehicle.vel.x * dt;
  17         float east_offset = vehicle.vel.y * dt;
  18         vehicle.location_estimate.offset(north_offset, east_offset);
  19         vehicle.location_estimate.alt += vehicle.vel.z * 100.0f * dt;
  20         // set valid_location flag
  21         vehicle.location_valid = true;
  22     } else {
  23         // vehicle has been lost, set lost flag
  24         vehicle.location_valid = false;
  25     }
  26 }
  27
  28 /**
  29   update_tracker_position - updates antenna tracker position from GPS location
  30   should be called at 50hz
  31  */
  32 void Tracker::update_tracker_position()
  33 {
  34     Location temp_loc;
  35
  36     // REVISIT: what if we lose lock during a mission and the antenna is moving?
  37     if (ahrs.get_location(temp_loc)) {
  38         stationary = false;
  39         current_loc = temp_loc;
  40     }
  41 }
  42
  43 /**
  44   update_bearing_and_distance - updates bearing and distance to the vehicle
  45   should be called at 50hz
  46  */
  47 void Tracker::update_bearing_and_distance()
  48 {
  49     // exit immediately if we do not have a valid vehicle position
  50     if (!vehicle.location_valid) {
  51         return;
  52     }
  53
  54     // calculate bearing to vehicle
  55     // To-Do: remove need for check of control_mode
  56     if (mode != &mode_scan && !nav_status.manual_control_yaw) {
  57         nav_status.bearing  = current_loc.get_bearing_to(vehicle.location_estimate) * 0.01f;
  58     }
  59
  60     // calculate distance to vehicle
  61     nav_status.distance = current_loc.get_distance(vehicle.location_estimate);
  62
  63     // calculate altitude difference to vehicle using gps
  64     if (g.alt_source == ALT_SOURCE_GPS){
  65         nav_status.alt_difference_gps = (vehicle.location_estimate.alt - current_loc.alt) / 100.0f;
  66     } else {
  67         // g.alt_source == ALT_SOURCE_GPS_VEH_ONLY
  68         nav_status.alt_difference_gps = vehicle.relative_alt / 100.0f;
  69     }
  70
  71     // calculate pitch to vehicle
  72     // To-Do: remove need for check of control_mode
  73     if (mode->number() != Mode::Number::SCAN && !nav_status.manual_control_pitch) {
  74         if (g.alt_source == ALT_SOURCE_BARO) {
  75             nav_status.pitch = degrees(atan2f(nav_status.alt_difference_baro, nav_status.distance));
  76         } else {
  77             nav_status.pitch = degrees(atan2f(nav_status.alt_difference_gps, nav_status.distance));
  78         }
  79     }
  80 }
  81
  82 /**
  83   main antenna tracking code, called at 50Hz
  84  */
  85 void Tracker::update_tracking(void)
  86 {
  87     // update vehicle position estimate
  88     update_vehicle_pos_estimate();
  89
  90     // update antenna tracker position from GPS
  91     update_tracker_position();
  92
  93     // update bearing and distance to vehicle
  94     update_bearing_and_distance();
  95
  96     // do not perform any servo updates until startup delay has passed
  97     if (g.startup_delay > 0 &&
  98         AP_HAL::millis() - start_time_ms < g.startup_delay*1000) {
  99         return;
 100     }
 101
 102     // do not perform updates if safety switch is disarmed (i.e. servos can't be moved)
 103     if (hal.util->safety_switch_state() == AP_HAL::Util::SAFETY_DISARMED) {
 104         return;
 105     }
 106     // do not move if we are not armed:
 107     if (!hal.util->get_soft_armed()) {
 108         switch ((PWMDisarmed)g.disarm_pwm.get()) {
 109         case PWMDisarmed::TRIM:
 110             SRV_Channels::set_output_scaled(SRV_Channel::k_tracker_yaw, 0);
 111             SRV_Channels::set_output_scaled(SRV_Channel::k_tracker_pitch, 0);
 112             break;
 113         default:
 114         case PWMDisarmed::ZERO:
 115             SRV_Channels::set_output_pwm(SRV_Channel::k_tracker_yaw, 0);
 116             SRV_Channels::set_output_pwm(SRV_Channel::k_tracker_pitch, 0);
 117             break;
 118         }
 119     } else {
 120         mode->update();
 121     }
 122
 123     // convert servo_out to radio_out and send to servo
 124     SRV_Channels::calc_pwm();
 125     SRV_Channels::output_ch_all();
 126     return;
 127 }
 128
 129 /**
 130    handle an updated position from the aircraft
 131  */
 132 void Tracker::tracking_update_position(const mavlink_global_position_int_t &msg)
 133 {
 134     vehicle.location.lat = msg.lat;
 135     vehicle.location.lng = msg.lon;
 136     vehicle.location.alt = msg.alt/10;
 137     vehicle.relative_alt = msg.relative_alt/10;
 138     vehicle.vel = Vector3f(msg.vx/100.0f, msg.vy/100.0f, msg.vz/100.0f);
 139     vehicle.last_update_us = AP_HAL::micros();
 140     vehicle.last_update_ms = AP_HAL::millis();
 141 #if HAL_LOGGING_ENABLED
 142     // log vehicle as GPS2
 143     if (should_log(MASK_LOG_GPS)) {
 144         Log_Write_Vehicle_Pos(vehicle.location.lat, vehicle.location.lng, vehicle.location.alt, vehicle.vel);
 145     }
 146 #endif
 147 }
 148
 149
 150 /**
 151    handle an updated pressure reading from the aircraft
 152  */
 153 void Tracker::tracking_update_pressure(const mavlink_scaled_pressure_t &msg)
 154 {
 155     float local_pressure = barometer.get_pressure();
 156     float aircraft_pressure = msg.press_abs*100.0f;
 157
 158     // calculate altitude difference based on difference in barometric pressure
 159     float alt_diff = barometer.get_altitude_difference(local_pressure, aircraft_pressure);
 160     if (!isnan(alt_diff) && !isinf(alt_diff)) {
 161         nav_status.alt_difference_baro = alt_diff + nav_status.altitude_offset;
 162
 163                 if (nav_status.need_altitude_calibration) {
 164                         // we have done a baro calibration - zero the altitude
 165                         // difference to the aircraft
 166                         nav_status.altitude_offset = -alt_diff;
 167                         nav_status.alt_difference_baro = 0;
 168                         nav_status.need_altitude_calibration = false;
 169                 }
 170     }
 171
 172 #if HAL_LOGGING_ENABLED
 173     // log vehicle baro data
 174     Log_Write_Vehicle_Baro(aircraft_pressure, alt_diff);
 175 #endif
 176 }
 177
 178 /**
 179    handle a manual control message by using the data to command yaw and pitch
 180  */
 181 void Tracker::tracking_manual_control(const mavlink_manual_control_t &msg)
 182 {
 183     nav_status.bearing = msg.x;
 184     nav_status.pitch   = msg.y;
 185     nav_status.distance = 0.0;
 186     nav_status.manual_control_yaw   = (msg.x != 0x7FFF);
 187     nav_status.manual_control_pitch = (msg.y != 0x7FFF);
 188     // z, r and buttons are not used
 189 }
 190
 191 /**
 192    update_armed_disarmed - set armed LED if we have received a position update within the last 5 seconds
 193  */
 194 void Tracker::update_armed_disarmed() const
 195 {
 196     if (vehicle.last_update_ms != 0 && (AP_HAL::millis() - vehicle.last_update_ms) < TRACKING_TIMEOUT_MS) {
 197         AP_Notify::flags.armed = true;
 198     } else {
 199         AP_Notify::flags.armed = false;
 200     }
 201 }
 202
 203 /*
 204   Returns the pan and tilt for use by onvif camera in scripting
 205   the output will be mapped to -1..1 from limits specified by PITCH_MIN
 206   and PITCH_MAX for tilt, and YAW_RANGE for pan
 207 */
 208 bool Tracker::get_pan_tilt_norm(float &pan_norm, float &tilt_norm) const
 209 {
 210     float pitch = nav_status.pitch;
 211     float bearing = nav_status.bearing;
 212     // set tilt value
 213     tilt_norm = (((constrain_float(pitch+g.pitch_trim, g.pitch_min, g.pitch_max) - g.pitch_min)*2.0f)/(g.pitch_max - g.pitch_min)) - 1;
 214     // set yaw value
 215     pan_norm = (wrap_360(bearing+g.yaw_trim)*2.0f/(g.yaw_range)) - 1;
 216     return true;
 217 }