mirror of https://github.com/ArduPilot/ardupilot
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.
This commit is contained in:
Mike McCauley
Andrew Tridgell
parent
8251cf32fb
commit
0885d55905
|
|
@ -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,
|
||||
0);
|
||||
MAV_STATE_ACTIVE);
|
||||
}
|
||||
|
||||
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
|
||||
loc.lng, // in 1E7 degrees
|
||||
g_gps->altitude_cm * 10, // millimeters above sea level
|
||||
current_loc.lat, // in 1E7 degrees
|
||||
current_loc.lng, // in 1E7 degrees
|
||||
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);
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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
|
||||
};
|
||||
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
||||
|
|
|
|||
|
|
@ -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();
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -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_pitch.set_angle(4500);
|
||||
channel_yaw.set_angle(18000); // Yaw is expected to drive antenna azimuth -180-0-180
|
||||
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;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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
|
||||
requested pitch
|
||||
update the pitch (elevation) servo. The aim is to drive the boards ahrs pitch to the
|
||||
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;
|
||||
our_location.lng = g_gps->longitude;
|
||||
our_location.alt = 0; // assume ground level for now
|
||||
current_loc.lat = g_gps->latitude;
|
||||
current_loc.lng = g_gps->longitude;
|
||||
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 distance = get_distance(our_location, vehicle.location);
|
||||
float pitch = degrees(atan2(vehicle.location.alt - our_location.alt, distance));
|
||||
|
||||
float bearing = get_bearing_cd(current_loc, vehicle.location) * 0.01f;
|
||||
float distance = get_distance(current_loc, vehicle.location);
|
||||
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();
|
||||
}
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue