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

AntennaTracker: add support for MANUAL and AUTO modes

This commit is contained in:
Mike McCauley 2014-03-07 08:13:53 +10:00 committed by Andrew Tridgell
parent 7c2bba169e
commit b4a0c7db63
5 changed files with 112 additions and 32 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
Tools/AntennaTracker/AntennaTracker.pde
View File

@ -192,6 +192,10 @@ static GCS_MAVLINK gcs[MAVLINK_COMM_NUM_BUFFERS];
////////////////////////////////////////////////////////////////////////////////
static struct Location current_loc;
// This is the state of the antenna control system
// There are multiple states defined such as MANUAL, FBW-A, AUTO
static enum ControlMode control_mode = INITIALISING;
/*
scheduler table - all regular tasks apart from the fast_loop()
should be listed here, along with how often they should be called

85
Tools/AntennaTracker/GCS_Mavlink.pde
View File

@ -24,13 +24,43 @@ static bool mavlink_active;
static NOINLINE void send_heartbeat(mavlink_channel_t chan)
{
uint8_t base_mode = MAV_MODE_FLAG_CUSTOM_MODE_ENABLED;
uint8_t system_status = MAV_STATE_ACTIVE;
uint32_t custom_mode = control_mode;
// work out the base_mode. This value is not very useful
// for APM, but we calculate it as best we can so a generic
// MAVLink enabled ground station can work out something about
// what the MAV is up to. The actual bit values are highly
// ambiguous for most of the APM flight modes. In practice, you
// only get useful information from the custom_mode, which maps to
// the APM flight mode and has a well defined meaning in the
// ArduPlane documentation
switch (control_mode) {
case MANUAL:
base_mode = MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
break;
case AUTO:
base_mode = MAV_MODE_FLAG_GUIDED_ENABLED |
MAV_MODE_FLAG_STABILIZE_ENABLED;
// note that MAV_MODE_FLAG_AUTO_ENABLED does not match what
// APM does in any mode, as that is defined as "system finds its own goal
// positions", which APM does not currently do
break;
case INITIALISING:
system_status = MAV_STATE_CALIBRATING;
break;
}
mavlink_msg_heartbeat_send(
chan,
MAV_TYPE_ANTENNA_TRACKER,
MAV_AUTOPILOT_ARDUPILOTMEGA,
MAV_MODE_FLAG_AUTO_ENABLED,
0,
MAV_STATE_ACTIVE);
base_mode,
custom_mode,
system_status);
}
static NOINLINE void send_attitude(mavlink_channel_t chan)
@ -631,8 +661,6 @@ 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:
@ -859,22 +887,47 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
break;
}
case MAV_CMD_COMPONENT_ARM_DISARM:
if (packet.target_component == MAV_COMP_ID_SYSTEM_CONTROL) {
if (packet.param1 == 1.0f) {
arm_servos();
result = MAV_RESULT_ACCEPTED;
} else if (packet.param1 == 0.0f) {
disarm_servos();
result = MAV_RESULT_ACCEPTED;
case MAV_CMD_COMPONENT_ARM_DISARM:
if (packet.target_component == MAV_COMP_ID_SYSTEM_CONTROL) {
if (packet.param1 == 1.0f) {
arm_servos();
result = MAV_RESULT_ACCEPTED;
} else if (packet.param1 == 0.0f) {
disarm_servos();
result = MAV_RESULT_ACCEPTED;
} else {
result = MAV_RESULT_UNSUPPORTED;
}
} else {
result = MAV_RESULT_UNSUPPORTED;
}
} else {
result = MAV_RESULT_UNSUPPORTED;
}
break;
case MAV_CMD_DO_SET_MODE:
switch ((uint16_t)packet.param1) {
case MAV_MODE_MANUAL_ARMED:
case MAV_MODE_MANUAL_DISARMED:
set_mode(MANUAL);
result = MAV_RESULT_ACCEPTED;
break;
case MAV_MODE_AUTO_ARMED:
case MAV_MODE_AUTO_DISARMED:
set_mode(AUTO);
result = MAV_RESULT_ACCEPTED;
break;
default:
result = MAV_RESULT_UNSUPPORTED;
}
break;
// mavproxy/mavutil sends this when auto command is entered
case MAV_CMD_MISSION_START:
set_mode(AUTO);
result = MAV_RESULT_ACCEPTED;
break;
case MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN:
{
if (packet.param1 == 1 || packet.param1 == 3) {

9
Tools/AntennaTracker/defines.h
View File

@ -50,5 +50,14 @@
#define MASK_OPTIONS_LOITER_DIRECTION (1<<2) // 0 = CW
// 1 = CCW
// Controller modes
// ----------------
enum ControlMode {
MANUAL=0,
AUTO=10,
INITIALISING=16
};
#endif // _DEFINES_H

12
Tools/AntennaTracker/system.pde
View File

@ -79,6 +79,8 @@ static void init_tracker()
gcs_send_text_P(SEVERITY_LOW,PSTR("\nReady to track."));
hal.scheduler->delay(1000); // Why????
set_mode(AUTO); // tracking
}
// Level the tracker by calibrating the INS
@ -203,3 +205,13 @@ static void disarm_servos()
channel_yaw.disable_out();
channel_pitch.disable_out();
}
static void set_mode(enum ControlMode mode)
{
if(control_mode == mode) {
// don't switch modes if we are already in the correct mode.
return;
}
control_mode = mode;
}

34
Tools/AntennaTracker/tracking.pde
View File

@ -172,24 +172,26 @@ static void update_tracking(void)
current_loc.options = 0; // Absolute altitude
}
// 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);
int32_t alt_diff_cm = current_loc.options & MASK_OPTIONS_RELATIVE_ALT // Do we know our absolute altitude?
? (vehicle.relative_alt - current_loc.alt)
: (vehicle.location.alt - current_loc.alt); // cm
float pitch = degrees(atan2(alt_diff_cm/100, distance));
// update the servos
update_pitch_servo(pitch);
update_yaw_servo(bearing);
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);
int32_t alt_diff_cm = current_loc.options & MASK_OPTIONS_RELATIVE_ALT // Do we know our absolute altitude?
? (vehicle.relative_alt - current_loc.alt)
: (vehicle.location.alt - current_loc.alt); // cm
float pitch = degrees(atan2(alt_diff_cm/100, 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;
}
/**
handle an updated position from the aircraft
*/