Line data    Source code

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