2015-06-01 02:04:25 -03:00
|
|
|
#include "Tracker.h"
|
|
|
|
|
2014-10-01 10:24:06 -03:00
|
|
|
/**
|
|
|
|
update_vehicle_position_estimate - updates estimate of vehicle positions
|
|
|
|
should be called at 50hz
|
|
|
|
*/
|
2015-06-01 02:04:25 -03:00
|
|
|
void Tracker::update_vehicle_pos_estimate()
|
2014-10-01 10:24:06 -03:00
|
|
|
{
|
|
|
|
// calculate time since last actual position update
|
2015-11-19 23:04:37 -04:00
|
|
|
float dt = (AP_HAL::micros() - vehicle.last_update_us) * 1.0e-6f;
|
2014-10-01 10:24:06 -03:00
|
|
|
|
|
|
|
// if less than 5 seconds since last position update estimate the position
|
|
|
|
if (dt < TRACKING_TIMEOUT_SEC) {
|
|
|
|
// project the vehicle position to take account of lost radio packets
|
|
|
|
vehicle.location_estimate = vehicle.location;
|
2016-06-13 05:50:41 -03:00
|
|
|
float north_offset = vehicle.vel.x * dt;
|
|
|
|
float east_offset = vehicle.vel.y * dt;
|
2019-02-24 20:11:04 -04:00
|
|
|
vehicle.location_estimate.offset(north_offset, east_offset);
|
2016-06-13 05:50:41 -03:00
|
|
|
vehicle.location_estimate.alt += vehicle.vel.z * 100.0f * dt;
|
2014-10-01 10:24:06 -03:00
|
|
|
// set valid_location flag
|
|
|
|
vehicle.location_valid = true;
|
|
|
|
} else {
|
|
|
|
// vehicle has been lost, set lost flag
|
|
|
|
vehicle.location_valid = false;
|
|
|
|
}
|
|
|
|
}
|
2014-03-22 05:09:01 -03:00
|
|
|
|
2014-10-01 11:00:00 -03:00
|
|
|
/**
|
|
|
|
update_tracker_position - updates antenna tracker position from GPS location
|
|
|
|
should be called at 50hz
|
|
|
|
*/
|
2015-06-01 02:04:25 -03:00
|
|
|
void Tracker::update_tracker_position()
|
2014-10-01 11:00:00 -03:00
|
|
|
{
|
2019-02-28 17:38:28 -04:00
|
|
|
Location temp_loc;
|
|
|
|
|
2014-10-01 11:00:00 -03:00
|
|
|
// REVISIT: what if we lose lock during a mission and the antenna is moving?
|
2019-02-28 17:38:28 -04:00
|
|
|
if (ahrs.get_position(temp_loc)) {
|
|
|
|
stationary = false;
|
|
|
|
current_loc = temp_loc;
|
2014-10-01 11:00:00 -03:00
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2014-10-01 10:45:22 -03:00
|
|
|
/**
|
|
|
|
update_bearing_and_distance - updates bearing and distance to the vehicle
|
|
|
|
should be called at 50hz
|
|
|
|
*/
|
2015-06-01 02:04:25 -03:00
|
|
|
void Tracker::update_bearing_and_distance()
|
2014-10-01 10:45:22 -03:00
|
|
|
{
|
|
|
|
// exit immediately if we do not have a valid vehicle position
|
|
|
|
if (!vehicle.location_valid) {
|
|
|
|
return;
|
|
|
|
}
|
|
|
|
|
|
|
|
// calculate bearing to vehicle
|
|
|
|
// To-Do: remove need for check of control_mode
|
|
|
|
if (control_mode != SCAN && !nav_status.manual_control_yaw) {
|
2019-04-05 10:02:44 -03:00
|
|
|
nav_status.bearing = current_loc.get_bearing_to(vehicle.location_estimate) * 0.01f;
|
2014-10-01 10:45:22 -03:00
|
|
|
}
|
|
|
|
|
|
|
|
// calculate distance to vehicle
|
2019-02-24 20:11:04 -04:00
|
|
|
nav_status.distance = current_loc.get_distance(vehicle.location_estimate);
|
2014-10-01 10:45:22 -03:00
|
|
|
|
2016-05-24 08:53:57 -03:00
|
|
|
// calculate altitude difference to vehicle using gps
|
2016-07-06 04:49:52 -03:00
|
|
|
if (g.alt_source == ALT_SOURCE_GPS){
|
|
|
|
nav_status.alt_difference_gps = (vehicle.location_estimate.alt - current_loc.alt) / 100.0f;
|
|
|
|
} else {
|
|
|
|
// g.alt_source == ALT_SOURCE_GPS_VEH_ONLY
|
|
|
|
nav_status.alt_difference_gps = vehicle.relative_alt / 100.0f;
|
|
|
|
}
|
2016-05-24 08:53:57 -03:00
|
|
|
|
2014-10-01 10:45:22 -03:00
|
|
|
// calculate pitch to vehicle
|
|
|
|
// To-Do: remove need for check of control_mode
|
|
|
|
if (control_mode != SCAN && !nav_status.manual_control_pitch) {
|
2016-06-13 05:44:23 -03:00
|
|
|
if (g.alt_source == ALT_SOURCE_BARO) {
|
2016-05-24 21:50:10 -03:00
|
|
|
nav_status.pitch = degrees(atan2f(nav_status.alt_difference_baro, nav_status.distance));
|
|
|
|
} else {
|
2016-05-24 08:53:57 -03:00
|
|
|
nav_status.pitch = degrees(atan2f(nav_status.alt_difference_gps, nav_status.distance));
|
|
|
|
}
|
2014-10-01 10:45:22 -03:00
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2013-10-13 04:14:13 -03:00
|
|
|
/**
|
|
|
|
main antenna tracking code, called at 50Hz
|
|
|
|
*/
|
2015-06-01 02:04:25 -03:00
|
|
|
void Tracker::update_tracking(void)
|
2013-10-13 04:14:13 -03:00
|
|
|
{
|
2014-10-01 10:24:06 -03:00
|
|
|
// update vehicle position estimate
|
|
|
|
update_vehicle_pos_estimate();
|
2013-10-13 04:14:13 -03:00
|
|
|
|
2014-10-01 11:00:00 -03:00
|
|
|
// update antenna tracker position from GPS
|
|
|
|
update_tracker_position();
|
2014-03-05 01:47:47 -04:00
|
|
|
|
2014-10-01 10:45:22 -03:00
|
|
|
// update bearing and distance to vehicle
|
|
|
|
update_bearing_and_distance();
|
2013-10-13 04:14:13 -03:00
|
|
|
|
2014-10-01 11:28:14 -03:00
|
|
|
// do not perform any servo updates until startup delay has passed
|
|
|
|
if (g.startup_delay > 0 &&
|
2015-11-19 23:04:37 -04:00
|
|
|
AP_HAL::millis() - start_time_ms < g.startup_delay*1000) {
|
2014-10-01 11:28:14 -03:00
|
|
|
return;
|
|
|
|
}
|
|
|
|
|
2014-10-11 05:06:29 -03:00
|
|
|
// do not perform updates if safety switch is disarmed (i.e. servos can't be moved)
|
|
|
|
if (hal.util->safety_switch_state() == AP_HAL::Util::SAFETY_DISARMED) {
|
|
|
|
return;
|
|
|
|
}
|
2019-02-28 17:38:28 -04:00
|
|
|
// do not move if we are not armed:
|
|
|
|
if (!hal.util->get_soft_armed()) {
|
|
|
|
return;
|
|
|
|
}
|
2014-10-11 05:06:29 -03:00
|
|
|
|
2014-03-22 05:09:01 -03:00
|
|
|
switch (control_mode) {
|
|
|
|
case AUTO:
|
2019-03-10 07:49:28 -03:00
|
|
|
if (vehicle.location_valid) {
|
|
|
|
update_auto();
|
|
|
|
} else if (tracker.target_set) {
|
|
|
|
update_scan();
|
|
|
|
}
|
2014-03-22 05:09:01 -03:00
|
|
|
break;
|
|
|
|
|
|
|
|
case MANUAL:
|
|
|
|
update_manual();
|
|
|
|
break;
|
|
|
|
|
2014-04-09 02:27:56 -03:00
|
|
|
case SCAN:
|
|
|
|
update_scan();
|
|
|
|
break;
|
|
|
|
|
2014-10-06 08:45:07 -03:00
|
|
|
case SERVO_TEST:
|
2014-03-22 05:09:01 -03:00
|
|
|
case STOP:
|
|
|
|
case INITIALISING:
|
|
|
|
break;
|
|
|
|
}
|
2013-10-13 04:14:13 -03:00
|
|
|
}
|
2014-03-12 00:36:51 -03:00
|
|
|
|
2013-10-13 04:14:13 -03:00
|
|
|
/**
|
|
|
|
handle an updated position from the aircraft
|
|
|
|
*/
|
2015-06-01 02:04:25 -03:00
|
|
|
void Tracker::tracking_update_position(const mavlink_global_position_int_t &msg)
|
2013-10-13 04:14:13 -03:00
|
|
|
{
|
|
|
|
vehicle.location.lat = msg.lat;
|
|
|
|
vehicle.location.lng = msg.lon;
|
2014-03-05 01:47:47 -04:00
|
|
|
vehicle.location.alt = msg.alt/10;
|
2016-07-06 04:49:52 -03:00
|
|
|
vehicle.relative_alt = msg.relative_alt/10;
|
2016-06-13 00:52:03 -03:00
|
|
|
vehicle.vel = Vector3f(msg.vx/100.0f, msg.vy/100.0f, msg.vz/100.0f);
|
2016-06-13 05:50:41 -03:00
|
|
|
vehicle.last_update_us = AP_HAL::micros();
|
2015-11-19 23:04:37 -04:00
|
|
|
vehicle.last_update_ms = AP_HAL::millis();
|
2015-12-27 03:05:14 -04:00
|
|
|
// log vehicle as GPS2
|
|
|
|
if (should_log(MASK_LOG_GPS)) {
|
2016-06-13 00:52:03 -03:00
|
|
|
Log_Write_Vehicle_Pos(vehicle.location.lat, vehicle.location.lng, vehicle.location.alt, vehicle.vel);
|
2015-12-27 03:05:14 -04:00
|
|
|
}
|
2013-10-13 04:14:13 -03:00
|
|
|
}
|
2014-03-02 03:00:37 -04:00
|
|
|
|
2014-03-12 00:36:51 -03:00
|
|
|
|
|
|
|
/**
|
|
|
|
handle an updated pressure reading from the aircraft
|
|
|
|
*/
|
2015-06-01 02:04:25 -03:00
|
|
|
void Tracker::tracking_update_pressure(const mavlink_scaled_pressure_t &msg)
|
2014-03-12 00:36:51 -03:00
|
|
|
{
|
|
|
|
float local_pressure = barometer.get_pressure();
|
|
|
|
float aircraft_pressure = msg.press_abs*100.0f;
|
|
|
|
|
|
|
|
// calculate altitude difference based on difference in barometric pressure
|
2014-04-11 03:34:14 -03:00
|
|
|
float alt_diff = barometer.get_altitude_difference(local_pressure, aircraft_pressure);
|
2016-06-02 21:56:58 -03:00
|
|
|
if (!isnan(alt_diff) && !isinf(alt_diff)) {
|
2016-05-24 05:31:59 -03:00
|
|
|
nav_status.alt_difference_baro = alt_diff + nav_status.altitude_offset;
|
2014-04-09 01:29:23 -03:00
|
|
|
|
2016-06-02 21:56:58 -03:00
|
|
|
if (nav_status.need_altitude_calibration) {
|
|
|
|
// we have done a baro calibration - zero the altitude
|
|
|
|
// difference to the aircraft
|
|
|
|
nav_status.altitude_offset = -alt_diff;
|
|
|
|
nav_status.alt_difference_baro = 0;
|
|
|
|
nav_status.need_altitude_calibration = false;
|
|
|
|
}
|
2014-03-12 00:36:51 -03:00
|
|
|
}
|
2016-05-12 02:17:31 -03:00
|
|
|
|
2016-06-09 03:06:53 -03:00
|
|
|
// log vehicle baro data
|
|
|
|
Log_Write_Vehicle_Baro(aircraft_pressure, alt_diff);
|
2014-03-12 00:36:51 -03:00
|
|
|
}
|
2014-03-15 19:17:04 -03:00
|
|
|
|
|
|
|
/**
|
|
|
|
handle a manual control message by using the data to command yaw and pitch
|
|
|
|
*/
|
2015-06-01 02:04:25 -03:00
|
|
|
void Tracker::tracking_manual_control(const mavlink_manual_control_t &msg)
|
2014-03-15 19:17:04 -03:00
|
|
|
{
|
|
|
|
nav_status.bearing = msg.x;
|
|
|
|
nav_status.pitch = msg.y;
|
|
|
|
nav_status.distance = 0.0;
|
2014-04-09 01:29:23 -03:00
|
|
|
nav_status.manual_control_yaw = (msg.x != 0x7FFF);
|
|
|
|
nav_status.manual_control_pitch = (msg.y != 0x7FFF);
|
2014-03-15 19:17:04 -03:00
|
|
|
// z, r and buttons are not used
|
|
|
|
}
|
2014-09-29 07:40:25 -03:00
|
|
|
|
|
|
|
/**
|
|
|
|
update_armed_disarmed - set armed LED if we have received a position update within the last 5 seconds
|
|
|
|
*/
|
2015-06-01 02:04:25 -03:00
|
|
|
void Tracker::update_armed_disarmed()
|
2014-09-29 07:40:25 -03:00
|
|
|
{
|
2015-11-19 23:04:37 -04:00
|
|
|
if (vehicle.last_update_ms != 0 && (AP_HAL::millis() - vehicle.last_update_ms) < TRACKING_TIMEOUT_MS) {
|
2014-09-29 07:40:25 -03:00
|
|
|
AP_Notify::flags.armed = true;
|
|
|
|
} else {
|
|
|
|
AP_Notify::flags.armed = false;
|
|
|
|
}
|
|
|
|
}
|