Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AntennaTracker: fixed ballerina and more accurate tracking 

implement new parameter options
This commit is contained in:
Andrew Tridgell 2014-03-22 19:09:01 +11:00
parent 262f8b9f48
commit 3a3a074fab
3 changed files with 144 additions and 56 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
Tools/AntennaTracker/AntennaTracker.pde
View File

@ -106,6 +106,7 @@ static struct {
float altitude_difference;
} nav_status;
static uint32_t start_time_ms;
////////////////////////////////////////////////////////////////////////////////
// prototypes

73
Tools/AntennaTracker/system.pde
View File

@ -73,14 +73,24 @@ static void init_tracker()
channel_yaw.calc_pwm();
channel_pitch.calc_pwm();
current_loc = get_home_eeprom(); // GPS may update this later
// use given start positions - useful for indoor testing, and
// while waiting for GPS lock
current_loc.lat = g.start_latitude * 1.0e7f;
current_loc.lng = g.start_longitude * 1.0e7f;
arm_servos();
// see if EEPROM has a default location as well
get_home_eeprom(current_loc);
gcs_send_text_P(SEVERITY_LOW,PSTR("\nReady to track."));
hal.scheduler->delay(1000); // Why????
set_mode(AUTO); // tracking
if (g.startup_delay > 0) {
// arm servos with trim value to allow them to start up (required
// for some servos)
prepare_servos();
}
}
// Level the tracker by calibrating the INS
@ -127,31 +137,30 @@ static uint32_t map_baudrate(int8_t rate, uint32_t default_baud)
/*
fetch HOME from EEPROM
*/
static struct Location get_home_eeprom()
static bool get_home_eeprom(struct Location &loc)
{
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));
}else{
// read WP position
mem = WP_START_BYTE;
temp.options = 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 false;
}
return temp;
// read WP position
mem = WP_START_BYTE;
loc.options = hal.storage->read_byte(mem);
mem++;
loc.alt = hal.storage->read_dword(mem);
mem += 4;
loc.lat = hal.storage->read_dword(mem);
mem += 4;
loc.lng = hal.storage->read_dword(mem);
return true;
}
static void set_home_eeprom(struct Location temp)
@ -182,7 +191,7 @@ static void set_home(struct Location temp)
}
static void arm_servos()
{
{
channel_yaw.enable_out();
channel_pitch.enable_out();
}
@ -193,6 +202,18 @@ static void disarm_servos()
channel_pitch.disable_out();
}
/*
setup servos to trim value after initialising
*/
static void prepare_servos()
{
start_time_ms = hal.scheduler->millis();
channel_yaw.radio_out = channel_yaw.radio_trim;
channel_pitch.radio_out = channel_pitch.radio_trim;
channel_yaw.output();
channel_pitch.output();
}
static void set_mode(enum ControlMode mode)
{
if(control_mode == mode) {
@ -200,5 +221,17 @@ static void set_mode(enum ControlMode mode)
return;
}
control_mode = mode;
switch (control_mode) {
case AUTO:
case MANUAL:
arm_servos();
break;
case STOP:
case INITIALISING:
disarm_servos();
break;
}
}

126
Tools/AntennaTracker/tracking.pde
View File

@ -41,6 +41,18 @@ static void update_pitch_servo(float pitch)
// 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);
// add slew rate limit
if (g.pitch_slew_time > 0.02f) {
uint16_t max_change = 0.02f * 18000 / g.yaw_slew_time;
if (max_change < 1) {
max_change = 1;
}
new_servo_out = constrain_float(new_servo_out,
channel_yaw.servo_out - max_change,
channel_yaw.servo_out + max_change);
}
channel_pitch.calc_pwm();
channel_pitch.output();
}
@ -93,14 +105,25 @@ static void update_yaw_servo(float yaw)
Use our current yawspeed to determine if we are moving in the
right direction
*/
static int8_t slew_dir = 0;
static int8_t slew_dir;
static uint32_t slew_start_ms;
int8_t new_slew_dir = slew_dir;
Vector3f omega = ahrs.get_gyro();
if (abs(channel_yaw.servo_out) == 18000 && abs(err) > margin && err * omega.z >= 0) {
Vector3f earth_rotation = ahrs.get_gyro() * ahrs.get_dcm_matrix();
bool making_progress;
if (slew_dir != 0) {
making_progress = (-slew_dir * earth_rotation.z >= 0);
} else {
making_progress = (err * earth_rotation.z >= 0);
}
if (abs(channel_yaw.servo_out) == 18000 &&
abs(err) > margin &&
making_progress &&
hal.scheduler->millis() - slew_start_ms > g.min_reverse_time*1000) {
// we are at the limit of the servo and are not moving in the
// right direction, so slew the other way
new_slew_dir = -channel_yaw.servo_out / 18000;
slew_start_ms = hal.scheduler->millis();
}
/*
@ -111,10 +134,13 @@ static void update_yaw_servo(float yaw)
new_slew_dir = 0;
}
#if 0
::printf("err=%d slew_dir=%d new_slew_dir=%d servo=%d\n",
err, slew_dir, new_slew_dir, channel_yaw.servo_out);
#endif
if (new_slew_dir != slew_dir) {
gcs_send_text_fmt(PSTR("SLEW: %d/%d err=%ld servo=%ld ez=%.3f"),
(int)slew_dir, (int)new_slew_dir,
(long)err,
(long)channel_yaw.servo_out,
degrees(ahrs.get_gyro().z));
}
slew_dir = new_slew_dir;
@ -128,28 +154,50 @@ static void update_yaw_servo(float yaw)
new_servo_out = constrain_float(channel_yaw.servo_out - servo_change, -18000, 18000);
}
if (new_servo_out - channel_yaw.servo_out > 100) {
new_servo_out = channel_yaw.servo_out + 100;
} else if (new_servo_out - channel_yaw.servo_out < -100) {
new_servo_out = channel_yaw.servo_out - 100;
// add slew rate limit
if (g.yaw_slew_time > 0.02f) {
uint16_t max_change = 0.02f * 36000.0f / g.yaw_slew_time;
if (max_change < 1) {
max_change = 1;
}
new_servo_out = constrain_float(new_servo_out,
channel_yaw.servo_out - max_change,
channel_yaw.servo_out + max_change);
}
channel_yaw.servo_out = new_servo_out;
{
// 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
}
channel_yaw.calc_pwm();
channel_yaw.output();
}
/*
control servos for AUTO mode
*/
static void update_auto(void)
{
if (g.startup_delay > 0 &&
hal.scheduler->millis() - start_time_ms < g.startup_delay*1000) {
return;
}
update_pitch_servo(nav_status.pitch);
update_yaw_servo(nav_status.bearing);
}
/*
control servos for MANUAL mode
*/
static void update_manual(void)
{
channel_yaw.radio_out = channel_yaw.radio_in;
channel_pitch.radio_out = channel_pitch.radio_in;
channel_yaw.output();
channel_pitch.output();
}
/**
main antenna tracking code, called at 50Hz
*/
@ -169,23 +217,29 @@ static void update_tracking(void)
current_loc.options = 0; // Absolute altitude
}
if (control_mode == AUTO)
{
// calculate the bearing to the vehicle
float bearing = get_bearing_cd(current_loc, vehicle.location) * 0.01f;
float distance = get_distance(current_loc, vehicle.location);
float pitch = degrees(atan2f(nav_status.altitude_difference, distance));
// calculate the bearing to the vehicle
float bearing = get_bearing_cd(current_loc, vehicle.location) * 0.01f;
float distance = get_distance(current_loc, vehicle.location);
float pitch = degrees(atan2f(nav_status.altitude_difference, distance));
// update the servos
update_pitch_servo(pitch);
update_yaw_servo(bearing);
// update nav_status for NAV_CONTROLLER_OUTPUT
nav_status.bearing = bearing;
nav_status.pitch = pitch;
nav_status.distance = distance;
// update nav_status for NAV_CONTROLLER_OUTPUT
nav_status.bearing = bearing;
nav_status.pitch = pitch;
nav_status.distance = distance;
switch (control_mode) {
case AUTO:
update_auto();
break;
case MANUAL:
update_manual();
break;
case STOP:
case INITIALISING:
break;
}
}
/**