From b4a0c7db63041435448d248ad8f55926b44f2ddb Mon Sep 17 00:00:00 2001 From: Mike McCauley Date: Fri, 7 Mar 2014 08:13:53 +1000 Subject: [PATCH] AntennaTracker: add support for MANUAL and AUTO modes --- Tools/AntennaTracker/AntennaTracker.pde | 4 ++ Tools/AntennaTracker/GCS_Mavlink.pde | 85 ++++++++++++++++++++----- Tools/AntennaTracker/defines.h | 9 +++ Tools/AntennaTracker/system.pde | 12 ++++ Tools/AntennaTracker/tracking.pde | 34 +++++----- 5 files changed, 112 insertions(+), 32 deletions(-) diff --git a/Tools/AntennaTracker/AntennaTracker.pde b/Tools/AntennaTracker/AntennaTracker.pde index d6b3b109ad..981800209e 100644 --- a/Tools/AntennaTracker/AntennaTracker.pde +++ b/Tools/AntennaTracker/AntennaTracker.pde @@ -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 diff --git a/Tools/AntennaTracker/GCS_Mavlink.pde b/Tools/AntennaTracker/GCS_Mavlink.pde index f7b76f3cc2..b1643a57cd 100644 --- a/Tools/AntennaTracker/GCS_Mavlink.pde +++ b/Tools/AntennaTracker/GCS_Mavlink.pde @@ -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,21 +887,46 @@ 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: { diff --git a/Tools/AntennaTracker/defines.h b/Tools/AntennaTracker/defines.h index 56f5edb46a..088a247be9 100644 --- a/Tools/AntennaTracker/defines.h +++ b/Tools/AntennaTracker/defines.h @@ -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 diff --git a/Tools/AntennaTracker/system.pde b/Tools/AntennaTracker/system.pde index fe659beb53..1e0d9286c6 100644 --- a/Tools/AntennaTracker/system.pde +++ b/Tools/AntennaTracker/system.pde @@ -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; +} + diff --git a/Tools/AntennaTracker/tracking.pde b/Tools/AntennaTracker/tracking.pde index bc19fc625e..3ac4ae5c45 100644 --- a/Tools/AntennaTracker/tracking.pde +++ b/Tools/AntennaTracker/tracking.pde @@ -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 */