From f8adea2ddd3690d9a1b183502f35169d6b14d0a5 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 1 Jun 2015 15:04:25 +1000 Subject: [PATCH] AntennaTracker: complete conversion to .cpp files --- AntennaTracker/AntennaTracker.cpp | 262 +++++--------------------- AntennaTracker/GCS_Mavlink.cpp | 122 ++++++------ AntennaTracker/Parameters.cpp | 22 ++- AntennaTracker/Tracker.h | 253 +++++++++++++++++++++++++ AntennaTracker/control_auto.cpp | 4 +- AntennaTracker/control_manual.cpp | 4 +- AntennaTracker/control_scan.cpp | 4 +- AntennaTracker/control_servo_test.cpp | 4 +- AntennaTracker/make.inc | 40 ++++ AntennaTracker/radio.cpp | 4 +- AntennaTracker/sensors.cpp | 16 +- AntennaTracker/servos.cpp | 28 +-- AntennaTracker/system.cpp | 52 +++-- AntennaTracker/tracking.cpp | 18 +- 14 files changed, 496 insertions(+), 337 deletions(-) create mode 100644 AntennaTracker/Tracker.h create mode 100644 AntennaTracker/make.inc diff --git a/AntennaTracker/AntennaTracker.cpp b/AntennaTracker/AntennaTracker.cpp index 9f139b857a..2796c5c1a4 100644 --- a/AntennaTracker/AntennaTracker.cpp +++ b/AntennaTracker/AntennaTracker.cpp @@ -20,192 +20,9 @@ along with this program. If not, see . */ -//////////////////////////////////////////////////////////////////////////////// -// Header includes -//////////////////////////////////////////////////////////////////////////////// +#include "Tracker.h" -#include -#include -#include - -#include -#include -#include -#include -#include -#include // ArduPilot GPS library -#include // ArduPilot barometer library -#include // ArduPilot Mega Magnetometer Library -#include // ArduPilot Mega Vector/Matrix math Library -#include // ArduPilot Mega Analog to Digital Converter Library -#include -#include // Inertial Sensor Library -#include // ArduPilot Mega DCM Library -#include // Filter library -#include // APM FIFO Buffer -#include - -#include // MAVLink GCS definitions -#include // Serial manager library -#include // ArduPilot Mega Declination Helper Library -#include -#include -#include -#include // main loop scheduler -#include - -#include -#include -#include -#include -#include // Notify library -#include // Battery monitor library -#include -#include -#include -#include -#include - -// Configuration -#include "config.h" - -// Local modules -#include "defines.h" - -#include "Parameters.h" -#include "GCS.h" - -#include -#include -#include -#include -#include -#include - -AP_HAL::BetterStream* cliSerial; - -const AP_HAL::HAL& hal = AP_HAL_BOARD_DRIVER; - -//////////////////////////////////////////////////////////////////////////////// -// the rate we run the main loop at -//////////////////////////////////////////////////////////////////////////////// -static const AP_InertialSensor::Sample_rate ins_sample_rate = AP_InertialSensor::RATE_50HZ; - -//////////////////////////////////////////////////////////////////////////////// -// Parameters -//////////////////////////////////////////////////////////////////////////////// -// -// Global parameters are all contained within the 'g' class. -// -static Parameters g; - -// main loop scheduler -static AP_Scheduler scheduler; - -// notification object for LEDs, buzzers etc -static AP_Notify notify; - -static uint32_t start_time_ms; - -static bool usb_connected; - -//////////////////////////////////////////////////////////////////////////////// -// prototypes -void gcs_send_text_fmt(const prog_char_t *fmt, ...); - - -//////////////////////////////////////////////////////////////////////////////// -// Sensors -//////////////////////////////////////////////////////////////////////////////// -static AP_GPS gps; - -static AP_Baro barometer; - -static Compass compass; - -#if CONFIG_HAL_BOARD == HAL_BOARD_APM1 -AP_ADC_ADS7844 apm1_adc; -#endif - -AP_InertialSensor ins; - -static RangeFinder rng; - -// Inertial Navigation EKF -#if AP_AHRS_NAVEKF_AVAILABLE -AP_AHRS_NavEKF ahrs(ins, barometer, gps, rng); -#else -AP_AHRS_DCM ahrs(ins, barometer, gps); -#endif - -#if CONFIG_HAL_BOARD == HAL_BOARD_SITL -SITL sitl; -#endif - -/** - antenna control channels - */ -static RC_Channel channel_yaw(CH_YAW); -static RC_Channel channel_pitch(CH_PITCH); - -//////////////////////////////////////////////////////////////////////////////// -// GCS selection -//////////////////////////////////////////////////////////////////////////////// -static AP_SerialManager serial_manager; -static const uint8_t num_gcs = MAVLINK_COMM_NUM_BUFFERS; -static GCS_MAVLINK gcs[MAVLINK_COMM_NUM_BUFFERS]; - -// board specific config -static AP_BoardConfig BoardConfig; - -//////////////////////////////////////////////////////////////////////////////// -// 3D Location vectors -// Location structure defined in AP_Common -//////////////////////////////////////////////////////////////////////////////// -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; - -//////////////////////////////////////////////////////////////////////////////// -// Vehicle state -//////////////////////////////////////////////////////////////////////////////// -static struct { - bool location_valid; // true if we have a valid location for the vehicle - Location location; // lat, long in degrees * 10^7; alt in meters * 100 - Location location_estimate; // lat, long in degrees * 10^7; alt in meters * 100 - uint32_t last_update_us; // last position update in micxroseconds - uint32_t last_update_ms; // last position update in milliseconds - float heading; // last known direction vehicle is moving - float ground_speed; // vehicle's last known ground speed in m/s -} vehicle; - -//////////////////////////////////////////////////////////////////////////////// -// Navigation controller state -//////////////////////////////////////////////////////////////////////////////// -static struct { - float bearing; // bearing to vehicle in centi-degrees - float distance; // distance to vehicle in meters - float pitch; // pitch to vehicle in degrees (positive means vehicle is above tracker, negative means below) - float altitude_difference; // altitude difference between tracker and vehicle in meters. positive value means vehicle is above tracker - float altitude_offset; // offset in meters which is added to tracker altitude to align altitude measurements with vehicle's barometer - bool manual_control_yaw : 1;// true if tracker yaw is under manual control - bool manual_control_pitch : 1;// true if tracker pitch is manually controlled - bool need_altitude_calibration : 1;// true if tracker altitude has not been determined (true after startup) - bool scan_reverse_pitch : 1;// controls direction of pitch movement in SCAN mode - bool scan_reverse_yaw : 1;// controls direction of yaw movement in SCAN mode -} nav_status; - -//////////////////////////////////////////////////////////////////////////////// -// Servo state -//////////////////////////////////////////////////////////////////////////////// -static struct { - bool yaw_lower : 1; // true if yaw servo has been limited from moving to a lower position (i.e. position or rate limited) - bool yaw_upper : 1; // true if yaw servo has been limited from moving to a higher position (i.e. position or rate limited) - bool pitch_lower : 1; // true if pitch servo has been limited from moving to a lower position (i.e. position or rate limited) - bool pitch_upper : 1; // true if pitch servo has been limited from moving to a higher position (i.e. position or rate limited) -} servo_limit; +#define SCHED_TASK(func) FUNCTOR_BIND_VOID(&tracker, &Tracker::func, void) /* scheduler table - all regular tasks apart from the fast_loop() @@ -213,33 +30,28 @@ static struct { (in 20ms units) and the maximum time they are expected to take (in microseconds) */ -static const AP_Scheduler::Task scheduler_tasks[] PROGMEM = { - { update_ahrs, 1, 1000 }, - { read_radio, 1, 200 }, - { update_tracking, 1, 1000 }, - { update_GPS, 5, 4000 }, - { update_compass, 5, 1500 }, - { update_barometer, 5, 1500 }, - { gcs_update, 1, 1700 }, - { gcs_data_stream_send, 1, 3000 }, - { compass_accumulate, 1, 1500 }, - { barometer_accumulate, 1, 900 }, - { update_notify, 1, 100 }, - { check_usb_mux, 5, 300 }, - { gcs_retry_deferred, 1, 1000 }, - { one_second_loop, 50, 3900 } +const AP_Scheduler::Task Tracker::scheduler_tasks[] PROGMEM = { + { SCHED_TASK(update_ahrs), 1, 1000 }, + { SCHED_TASK(read_radio), 1, 200 }, + { SCHED_TASK(update_tracking), 1, 1000 }, + { SCHED_TASK(update_GPS), 5, 4000 }, + { SCHED_TASK(update_compass), 5, 1500 }, + { SCHED_TASK(update_barometer), 5, 1500 }, + { SCHED_TASK(gcs_update), 1, 1700 }, + { SCHED_TASK(gcs_data_stream_send), 1, 3000 }, + { SCHED_TASK(compass_accumulate), 1, 1500 }, + { SCHED_TASK(barometer_accumulate), 1, 900 }, + { SCHED_TASK(update_notify), 1, 100 }, + { SCHED_TASK(check_usb_mux), 5, 300 }, + { SCHED_TASK(gcs_retry_deferred), 1, 1000 }, + { SCHED_TASK(one_second_loop), 50, 3900 } }; -// setup the var_info table -AP_Param param_loader(var_info); - /** setup the sketch - called once on startup */ -void setup() +void Tracker::setup() { - cliSerial = hal.console; - // load the default values of variables listed in var_info[] AP_Param::setup_sketch_defaults(); @@ -260,7 +72,7 @@ void setup() /** loop() is called continuously */ -void loop() +void Tracker::loop() { // wait for an INS sample ins.wait_for_sample(); @@ -271,7 +83,7 @@ void loop() scheduler.run(19900UL); } -static void one_second_loop() +void Tracker::one_second_loop() { // send a heartbeat gcs_send_message(MSG_HEARTBEAT); @@ -285,15 +97,41 @@ static void one_second_loop() // updated armed/disarmed status LEDs update_armed_disarmed(); - static uint8_t counter; - counter++; + one_second_counter++; - if (counter >= 60) { + if (one_second_counter >= 60) { if(g.compass_enabled) { compass.save_offsets(); } - counter = 0; + one_second_counter = 0; } } +#if CONFIG_HAL_BOARD == HAL_BOARD_APM1 +// needed for APM1 inertialsensor driver +AP_ADC_ADS7844 apm1_adc; +#endif + +const AP_HAL::HAL& hal = AP_HAL_BOARD_DRIVER; + +Tracker::Tracker(void) +{} + +Tracker tracker; + +/* + compatibility with old pde style build + */ +void setup(void); +void loop(void); + +void setup(void) +{ + tracker.setup(); +} +void loop(void) +{ + tracker.loop(); +} + AP_HAL_MAIN(); diff --git a/AntennaTracker/GCS_Mavlink.cpp b/AntennaTracker/GCS_Mavlink.cpp index a63c7f794c..5279d79669 100644 --- a/AntennaTracker/GCS_Mavlink.cpp +++ b/AntennaTracker/GCS_Mavlink.cpp @@ -1,5 +1,7 @@ // -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- +#include "Tracker.h" + // default sensors are present and healthy: gyro, accelerometer, barometer, rate_control, attitude_stabilization, yaw_position, altitude control, x/y position control, motor_control #define MAVLINK_SENSOR_PRESENT_DEFAULT (MAV_SYS_STATUS_SENSOR_3D_GYRO | MAV_SYS_STATUS_SENSOR_3D_ACCEL | MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE | MAV_SYS_STATUS_SENSOR_ANGULAR_RATE_CONTROL | MAV_SYS_STATUS_SENSOR_ATTITUDE_STABILIZATION | MAV_SYS_STATUS_SENSOR_YAW_POSITION | MAV_SYS_STATUS_SENSOR_Z_ALTITUDE_CONTROL | MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL | MAV_SYS_STATUS_SENSOR_MOTOR_OUTPUTS) @@ -19,7 +21,7 @@ static bool in_mavlink_delay; * pattern below when adding any new messages */ -static NOINLINE void send_heartbeat(mavlink_channel_t chan) +void Tracker::send_heartbeat(mavlink_channel_t chan) { uint8_t base_mode = MAV_MODE_FLAG_CUSTOM_MODE_ENABLED; uint8_t system_status = MAV_STATE_ACTIVE; @@ -65,7 +67,7 @@ static NOINLINE void send_heartbeat(mavlink_channel_t chan) system_status); } -static NOINLINE void send_attitude(mavlink_channel_t chan) +void Tracker::send_attitude(mavlink_channel_t chan) { Vector3f omega = ahrs.get_gyro(); mavlink_msg_attitude_send( @@ -80,7 +82,7 @@ static NOINLINE void send_attitude(mavlink_channel_t chan) } -static void NOINLINE send_location(mavlink_channel_t chan) +void Tracker::send_location(mavlink_channel_t chan) { uint32_t fix_time; if (gps.status() >= AP_GPS::GPS_OK_FIX_2D) { @@ -102,7 +104,7 @@ static void NOINLINE send_location(mavlink_channel_t chan) ahrs.yaw_sensor); } -static void NOINLINE send_radio_out(mavlink_channel_t chan) +void Tracker::send_radio_out(mavlink_channel_t chan) { mavlink_msg_servo_output_raw_send( chan, @@ -118,7 +120,7 @@ static void NOINLINE send_radio_out(mavlink_channel_t chan) hal.rcout->read(7)); } -static void NOINLINE send_hwstatus(mavlink_channel_t chan) +void Tracker::send_hwstatus(mavlink_channel_t chan) { mavlink_msg_hwstatus_send( chan, @@ -126,12 +128,12 @@ static void NOINLINE send_hwstatus(mavlink_channel_t chan) hal.i2c->lockup_count()); } -static void NOINLINE send_waypoint_request(mavlink_channel_t chan) +void Tracker::send_waypoint_request(mavlink_channel_t chan) { gcs[chan-MAVLINK_COMM_0].queued_waypoint_send(); } -static void NOINLINE send_statustext(mavlink_channel_t chan) +void Tracker::send_statustext(mavlink_channel_t chan) { mavlink_statustext_t *s = &gcs[chan-MAVLINK_COMM_0].pending_status; mavlink_msg_statustext_send( @@ -140,7 +142,7 @@ static void NOINLINE send_statustext(mavlink_channel_t chan) s->text); } -static void NOINLINE send_nav_controller_output(mavlink_channel_t chan) +void Tracker::send_nav_controller_output(mavlink_channel_t chan) { mavlink_msg_nav_controller_output_send( chan, @@ -156,7 +158,7 @@ static void NOINLINE send_nav_controller_output(mavlink_channel_t chan) // report simulator state -static void NOINLINE send_simstate(mavlink_channel_t chan) +void Tracker::send_simstate(mavlink_channel_t chan) { #if CONFIG_HAL_BOARD == HAL_BOARD_SITL sitl.simstate_send(chan); @@ -171,87 +173,87 @@ bool GCS_MAVLINK::try_send_message(enum ap_message id) switch (id) { case MSG_HEARTBEAT: CHECK_PAYLOAD_SIZE(HEARTBEAT); - send_heartbeat(chan); + tracker.send_heartbeat(chan); return true; case MSG_ATTITUDE: CHECK_PAYLOAD_SIZE(ATTITUDE); - send_attitude(chan); + tracker.send_attitude(chan); break; case MSG_LOCATION: CHECK_PAYLOAD_SIZE(GLOBAL_POSITION_INT); - send_location(chan); + tracker.send_location(chan); break; case MSG_LOCAL_POSITION: CHECK_PAYLOAD_SIZE(LOCAL_POSITION_NED); - send_local_position(ahrs); + send_local_position(tracker.ahrs); break; case MSG_NAV_CONTROLLER_OUTPUT: CHECK_PAYLOAD_SIZE(NAV_CONTROLLER_OUTPUT); - send_nav_controller_output(chan); + tracker.send_nav_controller_output(chan); break; case MSG_GPS_RAW: CHECK_PAYLOAD_SIZE(GPS_RAW_INT); - gcs[chan-MAVLINK_COMM_0].send_gps_raw(gps); + tracker.gcs[chan-MAVLINK_COMM_0].send_gps_raw(tracker.gps); break; case MSG_RADIO_IN: CHECK_PAYLOAD_SIZE(RC_CHANNELS_RAW); - gcs[chan-MAVLINK_COMM_0].send_radio_in(0); + tracker.gcs[chan-MAVLINK_COMM_0].send_radio_in(0); break; case MSG_RADIO_OUT: CHECK_PAYLOAD_SIZE(SERVO_OUTPUT_RAW); - send_radio_out(chan); + tracker.send_radio_out(chan); break; case MSG_RAW_IMU1: CHECK_PAYLOAD_SIZE(RAW_IMU); - gcs[chan-MAVLINK_COMM_0].send_raw_imu(ins, compass); + tracker.gcs[chan-MAVLINK_COMM_0].send_raw_imu(tracker.ins, tracker.compass); break; case MSG_RAW_IMU2: CHECK_PAYLOAD_SIZE(SCALED_PRESSURE); - gcs[chan-MAVLINK_COMM_0].send_scaled_pressure(barometer); + tracker.gcs[chan-MAVLINK_COMM_0].send_scaled_pressure(tracker.barometer); break; case MSG_RAW_IMU3: CHECK_PAYLOAD_SIZE(SENSOR_OFFSETS); - gcs[chan-MAVLINK_COMM_0].send_sensor_offsets(ins, compass, barometer); + tracker.gcs[chan-MAVLINK_COMM_0].send_sensor_offsets(tracker.ins, tracker.compass, tracker.barometer); break; case MSG_NEXT_PARAM: CHECK_PAYLOAD_SIZE(PARAM_VALUE); - gcs[chan-MAVLINK_COMM_0].queued_param_send(); + tracker.gcs[chan-MAVLINK_COMM_0].queued_param_send(); break; case MSG_NEXT_WAYPOINT: CHECK_PAYLOAD_SIZE(MISSION_REQUEST); - send_waypoint_request(chan); + tracker.send_waypoint_request(chan); break; case MSG_STATUSTEXT: CHECK_PAYLOAD_SIZE(STATUSTEXT); - send_statustext(chan); + tracker.send_statustext(chan); break; case MSG_AHRS: CHECK_PAYLOAD_SIZE(AHRS); - gcs[chan-MAVLINK_COMM_0].send_ahrs(ahrs); + tracker.gcs[chan-MAVLINK_COMM_0].send_ahrs(tracker.ahrs); break; case MSG_SIMSTATE: CHECK_PAYLOAD_SIZE(SIMSTATE); - send_simstate(chan); + tracker.send_simstate(chan); break; case MSG_HWSTATUS: CHECK_PAYLOAD_SIZE(HWSTATUS); - send_hwstatus(chan); + tracker.send_hwstatus(chan); break; case MSG_SERVO_OUT: @@ -456,7 +458,7 @@ GCS_MAVLINK::data_stream_send(void) We eavesdrop on MAVLINK_MSG_ID_GLOBAL_POSITION_INT and MAVLINK_MSG_ID_SCALED_PRESSUREs */ -void mavlink_snoop(const mavlink_message_t* msg) +void Tracker::mavlink_snoop(const mavlink_message_t* msg) { // return immediately if sysid doesn't match our target sysid if ((g.sysid_target != 0) && (g.sysid_target != msg->sysid)) { @@ -491,10 +493,8 @@ void mavlink_snoop(const mavlink_message_t* msg) } // locks onto a particular target sysid and sets it's position data stream to at least 1hz -void mavlink_check_target(const mavlink_message_t* msg) +void Tracker::mavlink_check_target(const mavlink_message_t* msg) { - static bool target_set = false; - // exit immediately if the target has already been set if (target_set) { return; @@ -587,34 +587,34 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) case MAV_CMD_PREFLIGHT_CALIBRATION: { if (is_equal(packet.param1,1.0f)) { - ins.init_gyro(); - if (ins.gyro_calibrated_ok_all()) { - ahrs.reset_gyro_drift(); + tracker.ins.init_gyro(); + if (tracker.ins.gyro_calibrated_ok_all()) { + tracker.ahrs.reset_gyro_drift(); result = MAV_RESULT_ACCEPTED; } else { result = MAV_RESULT_FAILED; } } if (is_equal(packet.param3,1.0f)) { - init_barometer(); + tracker.init_barometer(); // zero the altitude difference on next baro update - nav_status.need_altitude_calibration = true; + tracker.nav_status.need_altitude_calibration = true; } if (is_equal(packet.param4,1.0f)) { // Cant trim radio } else if (is_equal(packet.param5,1.0f)) { float trim_roll, trim_pitch; AP_InertialSensor_UserInteract_MAVLink interact(this); - if(ins.calibrate_accel(&interact, trim_roll, trim_pitch)) { + if(tracker.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)); + tracker.ahrs.set_trim(Vector3f(trim_roll, trim_pitch, 0)); } } else if (is_equal(packet.param5,2.0f)) { // accel trim float trim_roll, trim_pitch; - if(ins.calibrate_trim(trim_roll, trim_pitch)) { + if(tracker.ins.calibrate_trim(trim_roll, trim_pitch)) { // reset ahrs's trim to suggested values from calibration routine - ahrs.set_trim(Vector3f(trim_roll, trim_pitch, 0)); + tracker.ahrs.set_trim(Vector3f(trim_roll, trim_pitch, 0)); result = MAV_RESULT_ACCEPTED; } else { result = MAV_RESULT_FAILED; @@ -627,10 +627,10 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) case MAV_CMD_COMPONENT_ARM_DISARM: if (packet.target_component == MAV_COMP_ID_SYSTEM_CONTROL) { if (is_equal(packet.param1,1.0f)) { - arm_servos(); + tracker.arm_servos(); result = MAV_RESULT_ACCEPTED; } else if (is_zero(packet.param1)) { - disarm_servos(); + tracker.disarm_servos(); result = MAV_RESULT_ACCEPTED; } else { result = MAV_RESULT_UNSUPPORTED; @@ -644,13 +644,13 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) switch ((uint16_t)packet.param1) { case MAV_MODE_MANUAL_ARMED: case MAV_MODE_MANUAL_DISARMED: - set_mode(MANUAL); + tracker.set_mode(MANUAL); result = MAV_RESULT_ACCEPTED; break; case MAV_MODE_AUTO_ARMED: case MAV_MODE_AUTO_DISARMED: - set_mode(AUTO); + tracker.set_mode(AUTO); result = MAV_RESULT_ACCEPTED; break; @@ -660,14 +660,14 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) break; case MAV_CMD_DO_SET_SERVO: - if (servo_test_set_servo(packet.param1, packet.param2)) { + if (tracker.servo_test_set_servo(packet.param1, packet.param2)) { result = MAV_RESULT_ACCEPTED; } break; // mavproxy/mavutil sends this when auto command is entered case MAV_CMD_MISSION_START: - set_mode(AUTO); + tracker.set_mode(AUTO); result = MAV_RESULT_ACCEPTED; break; @@ -683,7 +683,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) case MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES: { if (is_equal(packet.param1,1.0f)) { - gcs[chan-MAVLINK_COMM_0].send_autopilot_version(); + tracker.gcs[chan-MAVLINK_COMM_0].send_autopilot_version(); result = MAV_RESULT_ACCEPTED; } break; @@ -789,7 +789,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) // check if this is the HOME wp if (packet.seq == 0) { - set_home(tell_command); // New home in EEPROM + tracker.set_home(tell_command); // New home in EEPROM send_text_P(SEVERITY_LOW,PSTR("new HOME received")); waypoint_receiving = false; } @@ -808,7 +808,7 @@ mission_failed: { mavlink_manual_control_t packet; mavlink_msg_manual_control_decode(msg, &packet); - tracking_manual_control(packet); + tracker.tracking_manual_control(packet); break; } @@ -817,7 +817,7 @@ mission_failed: // decode mavlink_global_position_int_t packet; mavlink_msg_global_position_int_decode(msg, &packet); - tracking_update_position(packet); + tracker.tracking_update_position(packet); break; } @@ -826,29 +826,29 @@ mission_failed: // decode mavlink_scaled_pressure_t packet; mavlink_msg_scaled_pressure_decode(msg, &packet); - tracking_update_pressure(packet); + tracker.tracking_update_pressure(packet); break; } case MAVLINK_MSG_ID_SET_MODE: { - handle_set_mode(msg, mavlink_set_mode); + handle_set_mode(msg, FUNCTOR_BIND(&tracker, &Tracker::mavlink_set_mode, bool, uint8_t)); break; } #if HAL_CPU_CLASS > HAL_CPU_CLASS_16 case MAVLINK_MSG_ID_SERIAL_CONTROL: - handle_serial_control(msg, gps); + handle_serial_control(msg, tracker.gps); break; case MAVLINK_MSG_ID_GPS_INJECT_DATA: - handle_gps_inject(msg, gps); + handle_gps_inject(msg, tracker.gps); break; #endif case MAVLINK_MSG_ID_AUTOPILOT_VERSION_REQUEST: - gcs[chan-MAVLINK_COMM_0].send_autopilot_version(); + tracker.gcs[chan-MAVLINK_COMM_0].send_autopilot_version(); break; } // end switch @@ -861,7 +861,7 @@ mission_failed: * MAVLink to process packets while waiting for the initialisation to * complete */ -static void mavlink_delay_cb() +void Tracker::mavlink_delay_cb() { static uint32_t last_1hz, last_50hz, last_5s; if (!gcs[0].initialised) return; @@ -890,7 +890,7 @@ static void mavlink_delay_cb() /* * send a message on both GCS links */ -static void gcs_send_message(enum ap_message id) +void Tracker::gcs_send_message(enum ap_message id) { for (uint8_t i=0; iprintf_P(PSTR("Firmware change: erasing EEPROM...\n")); + hal.console->printf_P(PSTR("Firmware change: erasing EEPROM...\n")); AP_Param::erase_all(); // save the current format version g.format_version.set_and_save(Parameters::k_format_version); - cliSerial->println_P(PSTR("done.")); + hal.console->println_P(PSTR("done.")); } else { uint32_t before = hal.scheduler->micros(); // Load all auto-loaded EEPROM variables AP_Param::load_all(); - cliSerial->printf_P(PSTR("load_all took %luus\n"), hal.scheduler->micros() - before); + hal.console->printf_P(PSTR("load_all took %luus\n"), (unsigned long)(hal.scheduler->micros() - before)); } } diff --git a/AntennaTracker/Tracker.h b/AntennaTracker/Tracker.h new file mode 100644 index 0000000000..c9b16438c8 --- /dev/null +++ b/AntennaTracker/Tracker.h @@ -0,0 +1,253 @@ +/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- + +#define THISFIRMWARE "AntennaTracker V0.7" +/* + Lead developers: Matthew Ridley and Andrew Tridgell + + Please contribute your ideas! See http://dev.ardupilot.com for details + + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ + +//////////////////////////////////////////////////////////////////////////////// +// Header includes +//////////////////////////////////////////////////////////////////////////////// + +#include +#include +#include + +#include +#include +#include +#include +#include +#include // ArduPilot GPS library +#include // ArduPilot barometer library +#include // ArduPilot Mega Magnetometer Library +#include // ArduPilot Mega Vector/Matrix math Library +#include // ArduPilot Mega Analog to Digital Converter Library +#include +#include // Inertial Sensor Library +#include // ArduPilot Mega DCM Library +#include // Filter library +#include // APM FIFO Buffer +#include + +#include // MAVLink GCS definitions +#include // Serial manager library +#include // ArduPilot Mega Declination Helper Library +#include +#include +#include +#include // main loop scheduler +#include + +#include +#include +#include +#include +#include // Notify library +#include // Battery monitor library +#include +#include +#include +#include +#include + +// Configuration +#include "config.h" +#include "defines.h" + +#include "Parameters.h" +#include "GCS.h" + +#include +#include +#include +#include +#include +#include + +class Tracker { +public: + friend class GCS_MAVLINK; + friend class Parameters; + + Tracker(void); + void setup(); + void loop(); + +private: + const AP_InertialSensor::Sample_rate ins_sample_rate = AP_InertialSensor::RATE_50HZ; + + Parameters g; + + // main loop scheduler + AP_Scheduler scheduler; + + // notification object for LEDs, buzzers etc + AP_Notify notify; + + uint32_t start_time_ms; + + bool usb_connected; + + AP_GPS gps; + + AP_Baro barometer; + + Compass compass; + + AP_InertialSensor ins; + + RangeFinder rng; + +// Inertial Navigation EKF +#if AP_AHRS_NAVEKF_AVAILABLE + AP_AHRS_NavEKF ahrs{ins, barometer, gps, rng}; +#else + AP_AHRS_DCM ahrs{ins, barometer, gps}; +#endif + +#if CONFIG_HAL_BOARD == HAL_BOARD_SITL + SITL sitl; +#endif + + /** + antenna control channels + */ + RC_Channel channel_yaw{CH_YAW}; + RC_Channel channel_pitch{CH_PITCH}; + + AP_SerialManager serial_manager; + const uint8_t num_gcs = MAVLINK_COMM_NUM_BUFFERS; + GCS_MAVLINK gcs[MAVLINK_COMM_NUM_BUFFERS]; + + AP_BoardConfig BoardConfig; + + struct Location current_loc; + + enum ControlMode control_mode = INITIALISING; + + // Vehicle state + struct { + bool location_valid; // true if we have a valid location for the vehicle + Location location; // lat, long in degrees * 10^7; alt in meters * 100 + Location location_estimate; // lat, long in degrees * 10^7; alt in meters * 100 + uint32_t last_update_us; // last position update in micxroseconds + uint32_t last_update_ms; // last position update in milliseconds + float heading; // last known direction vehicle is moving + float ground_speed; // vehicle's last known ground speed in m/s + } vehicle; + + // Navigation controller state + struct { + float bearing; // bearing to vehicle in centi-degrees + float distance; // distance to vehicle in meters + float pitch; // pitch to vehicle in degrees (positive means vehicle is above tracker, negative means below) + float altitude_difference; // altitude difference between tracker and vehicle in meters. positive value means vehicle is above tracker + float altitude_offset; // offset in meters which is added to tracker altitude to align altitude measurements with vehicle's barometer + bool manual_control_yaw : 1;// true if tracker yaw is under manual control + bool manual_control_pitch : 1;// true if tracker pitch is manually controlled + bool need_altitude_calibration : 1;// true if tracker altitude has not been determined (true after startup) + bool scan_reverse_pitch : 1;// controls direction of pitch movement in SCAN mode + bool scan_reverse_yaw : 1;// controls direction of yaw movement in SCAN mode + } nav_status; + + // Servo state + struct { + bool yaw_lower : 1; // true if yaw servo has been limited from moving to a lower position (i.e. position or rate limited) + bool yaw_upper : 1; // true if yaw servo has been limited from moving to a higher position (i.e. position or rate limited) + bool pitch_lower : 1; // true if pitch servo has been limited from moving to a lower position (i.e. position or rate limited) + bool pitch_upper : 1; // true if pitch servo has been limited from moving to a higher position (i.e. position or rate limited) + } servo_limit; + + // setup the var_info table + AP_Param param_loader{var_info}; + + uint8_t one_second_counter = 0; + bool target_set = false; + int8_t slew_dir; + uint32_t slew_start_ms; + + static const AP_Scheduler::Task scheduler_tasks[]; + static const AP_Param::Info var_info[]; + + void one_second_loop(); + void send_heartbeat(mavlink_channel_t chan); + void send_attitude(mavlink_channel_t chan); + void send_location(mavlink_channel_t chan); + void send_radio_out(mavlink_channel_t chan); + void send_hwstatus(mavlink_channel_t chan); + void send_waypoint_request(mavlink_channel_t chan); + void send_statustext(mavlink_channel_t chan); + void send_nav_controller_output(mavlink_channel_t chan); + void send_simstate(mavlink_channel_t chan); + void mavlink_check_target(const mavlink_message_t* msg); + void gcs_send_message(enum ap_message id); + void gcs_data_stream_send(void); + void gcs_update(void); + void gcs_send_text_P(gcs_severity severity, const prog_char_t *str); + void gcs_retry_deferred(void); + void load_parameters(void); + void update_auto(void); + void update_manual(void); + void update_scan(void); + bool servo_test_set_servo(uint8_t servo_num, uint16_t pwm); + void read_radio(); + void init_barometer(void); + void update_barometer(void); + void update_ahrs(); + void update_compass(void); + void compass_accumulate(void); + void barometer_accumulate(void); + void update_GPS(void); + void init_servos(); + void update_pitch_servo(float pitch); + void update_pitch_position_servo(float pitch); + void update_pitch_onoff_servo(float pitch); + void update_yaw_servo(float yaw); + void update_yaw_position_servo(float yaw); + void update_yaw_onoff_servo(float yaw); + void init_tracker(); + void update_notify(); + bool get_home_eeprom(struct Location &loc); + void set_home_eeprom(struct Location temp); + void set_home(struct Location temp); + void arm_servos(); + void disarm_servos(); + void prepare_servos(); + void set_mode(enum ControlMode mode); + bool mavlink_set_mode(uint8_t mode); + void check_usb_mux(void); + void update_vehicle_pos_estimate(); + void update_tracker_position(); + void update_bearing_and_distance(); + void update_tracking(void); + void tracking_update_position(const mavlink_global_position_int_t &msg); + void tracking_update_pressure(const mavlink_scaled_pressure_t &msg); + void tracking_manual_control(const mavlink_manual_control_t &msg); + void update_armed_disarmed(); + void gcs_send_text_fmt(const prog_char_t *fmt, ...); + +public: + void mavlink_snoop(const mavlink_message_t* msg); + void mavlink_delay_cb(); +}; + +#define MENU_FUNC(func) FUNCTOR_BIND(&tracker, &Tracker::func, int8_t, uint8_t, const Menu::arg *) + +extern const AP_HAL::HAL& hal; +extern Tracker tracker; diff --git a/AntennaTracker/control_auto.cpp b/AntennaTracker/control_auto.cpp index 7905692585..cc39b3f54e 100644 --- a/AntennaTracker/control_auto.cpp +++ b/AntennaTracker/control_auto.cpp @@ -1,5 +1,7 @@ // -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- +#include "Tracker.h" + /* * control_auto.pde - auto control mode */ @@ -8,7 +10,7 @@ * update_auto - runs the auto controller * called at 50hz while control_mode is 'AUTO' */ -static void update_auto(void) +void Tracker::update_auto(void) { // exit immediately if we do not have a valid vehicle position if (!vehicle.location_valid) { diff --git a/AntennaTracker/control_manual.cpp b/AntennaTracker/control_manual.cpp index 3d56f234d4..4ce72a3861 100644 --- a/AntennaTracker/control_manual.cpp +++ b/AntennaTracker/control_manual.cpp @@ -1,5 +1,7 @@ // -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- +#include "Tracker.h" + /* * control_manual.pde - manual control mode */ @@ -8,7 +10,7 @@ * update_manual - runs the manual controller * called at 50hz while control_mode is 'MANUAL' */ -static void update_manual(void) +void Tracker::update_manual(void) { // copy yaw and pitch input to output channel_yaw.radio_out = constrain_int16(channel_yaw.radio_in, channel_yaw.radio_min, channel_yaw.radio_max); diff --git a/AntennaTracker/control_scan.cpp b/AntennaTracker/control_scan.cpp index 15e7d1c7db..dd6e695785 100644 --- a/AntennaTracker/control_scan.cpp +++ b/AntennaTracker/control_scan.cpp @@ -1,5 +1,7 @@ // -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- +#include "Tracker.h" + /* * control_scan.pde - scan control mode */ @@ -8,7 +10,7 @@ * update_scan - runs the scan controller * called at 50hz while control_mode is 'SCAN' */ -static void update_scan(void) +void Tracker::update_scan(void) { if (!nav_status.manual_control_yaw) { float yaw_delta = g.scan_speed * 0.02f; diff --git a/AntennaTracker/control_servo_test.cpp b/AntennaTracker/control_servo_test.cpp index 123c60564d..cbee7088e5 100644 --- a/AntennaTracker/control_servo_test.cpp +++ b/AntennaTracker/control_servo_test.cpp @@ -1,5 +1,7 @@ // -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- +#include "Tracker.h" + /* * control_servo_test.pde - GCS controlled servo test mode */ @@ -8,7 +10,7 @@ * servo_test_set_servo - sets the yaw or pitch servo pwm directly * servo_num are 1 for yaw, 2 for pitch */ -static bool servo_test_set_servo(uint8_t servo_num, uint16_t pwm) +bool Tracker::servo_test_set_servo(uint8_t servo_num, uint16_t pwm) { // convert servo_num from 1~2 to 0~1 range servo_num--; diff --git a/AntennaTracker/make.inc b/AntennaTracker/make.inc new file mode 100644 index 0000000000..614a8c4e15 --- /dev/null +++ b/AntennaTracker/make.inc @@ -0,0 +1,40 @@ +LIBRARIES += AP_Common +LIBRARIES += AP_Progmem +LIBRARIES += AP_HAL +LIBRARIES += AP_Param +LIBRARIES += StorageManager +LIBRARIES += AP_GPS +LIBRARIES += AP_Baro +LIBRARIES += AP_Compass +LIBRARIES += AP_Math +LIBRARIES += AP_ADC +LIBRARIES += AP_ADC_AnalogSource +LIBRARIES += AP_InertialSensor +LIBRARIES += AP_AHRS +LIBRARIES += Filter +LIBRARIES += AP_Buffer +LIBRARIES += GCS_MAVLink +LIBRARIES += AP_SerialManager +LIBRARIES += AP_Declination +LIBRARIES += DataFlash +LIBRARIES += SITL +LIBRARIES += PID +LIBRARIES += AP_Scheduler +LIBRARIES += AP_NavEKF +LIBRARIES += AP_Vehicle +LIBRARIES += AP_Mission +LIBRARIES += AP_Terrain +LIBRARIES += AP_Rally +LIBRARIES += AP_Notify +LIBRARIES += AP_BattMonitor +LIBRARIES += AP_Airspeed +LIBRARIES += RC_Channel +LIBRARIES += AP_BoardConfig +LIBRARIES += AP_OpticalFlow +LIBRARIES += AP_RangeFinder +LIBRARIES += AP_HAL_AVR +LIBRARIES += AP_HAL_SITL +LIBRARIES += AP_HAL_PX4 +LIBRARIES += AP_HAL_FLYMAPLE +LIBRARIES += AP_HAL_Linux +LIBRARIES += AP_HAL_Empty diff --git a/AntennaTracker/radio.cpp b/AntennaTracker/radio.cpp index 81cb2b83d5..0bf301a588 100644 --- a/AntennaTracker/radio.cpp +++ b/AntennaTracker/radio.cpp @@ -1,8 +1,10 @@ // -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- +#include "Tracker.h" + // Functions to read the RC radio input -static void read_radio() +void Tracker::read_radio() { if (hal.rcin->new_input()) { channel_yaw.set_pwm(hal.rcin->read(CH_YAW)); diff --git a/AntennaTracker/sensors.cpp b/AntennaTracker/sensors.cpp index 55bc647c27..9234033631 100644 --- a/AntennaTracker/sensors.cpp +++ b/AntennaTracker/sensors.cpp @@ -1,6 +1,8 @@ // -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- -static void init_barometer(void) +#include "Tracker.h" + +void Tracker::init_barometer(void) { gcs_send_text_P(SEVERITY_LOW, PSTR("Calibrating barometer")); barometer.calibrate(); @@ -8,7 +10,7 @@ static void init_barometer(void) } // read the barometer and return the updated altitude in meters -static void update_barometer(void) +void Tracker::update_barometer(void) { barometer.update(); } @@ -17,7 +19,7 @@ static void update_barometer(void) /* update INS and attitude */ -static void update_ahrs() +void Tracker::update_ahrs() { ahrs.update(); } @@ -26,7 +28,7 @@ static void update_ahrs() /* read and update compass */ -static void update_compass(void) +void Tracker::update_compass(void) { if (g.compass_enabled && compass.read()) { ahrs.set_compass(&compass); @@ -39,7 +41,7 @@ static void update_compass(void) /* if the compass is enabled then try to accumulate a reading */ -static void compass_accumulate(void) +void Tracker::compass_accumulate(void) { if (g.compass_enabled) { compass.accumulate(); @@ -49,7 +51,7 @@ static void compass_accumulate(void) /* try to accumulate a baro reading */ -static void barometer_accumulate(void) +void Tracker::barometer_accumulate(void) { barometer.accumulate(); } @@ -58,7 +60,7 @@ static void barometer_accumulate(void) /* read the GPS */ -static void update_GPS(void) +void Tracker::update_GPS(void) { gps.update(); diff --git a/AntennaTracker/servos.cpp b/AntennaTracker/servos.cpp index ef77bceec1..fdd4d1284e 100644 --- a/AntennaTracker/servos.cpp +++ b/AntennaTracker/servos.cpp @@ -1,11 +1,13 @@ // -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- +#include "Tracker.h" + /* * servos.pde - code to move pitch and yaw servos to attain a target heading or pitch */ // init_servos - initialises the servos -static void init_servos() +void Tracker::init_servos() { // setup antenna control PWM channels channel_yaw.set_angle(g.yaw_range * 100/2); // yaw range is +/- (YAW_RANGE parameter/2) converted to centi-degrees @@ -24,7 +26,7 @@ static void init_servos() 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) +void Tracker::update_pitch_servo(float pitch) { switch ((enum ServoType)g.servo_type.get()) { case SERVO_TYPE_ONOFF: @@ -46,7 +48,7 @@ static void update_pitch_servo(float 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_position_servo(float pitch) +void Tracker::update_pitch_position_servo(float pitch) { // degrees(ahrs.pitch) is -90 to 90, where 0 is horizontal // pitch argument is -90 to 90, where 0 is horizontal @@ -111,7 +113,7 @@ static void update_pitch_position_servo(float 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_onoff_servo(float pitch) +void Tracker::update_pitch_onoff_servo(float pitch) { // degrees(ahrs.pitch) is -90 to 90, where 0 is horizontal // pitch argument is -90 to 90, where 0 is horizontal @@ -136,7 +138,7 @@ static void update_pitch_onoff_servo(float pitch) /** update the yaw (azimuth) servo. */ -static void update_yaw_servo(float yaw) +void Tracker::update_yaw_servo(float yaw) { switch ((enum ServoType)g.servo_type.get()) { case SERVO_TYPE_ONOFF: @@ -159,7 +161,7 @@ static void update_yaw_servo(float yaw) yaw to the requested yaw, so the board (and therefore the antenna) will be pointing at the target */ -static void update_yaw_position_servo(float yaw) +void Tracker::update_yaw_position_servo(float yaw) { int32_t ahrs_yaw_cd = wrap_180_cd(ahrs.yaw_sensor); int32_t yaw_cd = wrap_180_cd(yaw*100); @@ -195,8 +197,6 @@ static void update_yaw_position_servo(float yaw) Use our current yawspeed to determine if we are moving in the right direction */ - static int8_t slew_dir; - static uint32_t slew_start_ms; int8_t new_slew_dir = slew_dir; // get earth frame z-axis rotation rate in radians @@ -230,11 +230,11 @@ static void update_yaw_position_servo(float yaw) } 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)angle_err, - (long)channel_yaw.servo_out, - (double)degrees(ahrs.get_gyro().z)); + tracker.gcs_send_text_fmt(PSTR("SLEW: %d/%d err=%ld servo=%ld ez=%.3f"), + (int)slew_dir, (int)new_slew_dir, + (long)angle_err, + (long)channel_yaw.servo_out, + (double)degrees(ahrs.get_gyro().z)); } slew_dir = new_slew_dir; @@ -287,7 +287,7 @@ static void update_yaw_position_servo(float yaw) yaw to the requested yaw, so the board (and therefore the antenna) will be pointing at the target */ -static void update_yaw_onoff_servo(float yaw) +void Tracker::update_yaw_onoff_servo(float yaw) { int32_t ahrs_yaw_cd = wrap_180_cd(ahrs.yaw_sensor); int32_t yaw_cd = wrap_180_cd(yaw*100); diff --git a/AntennaTracker/system.cpp b/AntennaTracker/system.cpp index c6dbca783f..ca5190db21 100644 --- a/AntennaTracker/system.cpp +++ b/AntennaTracker/system.cpp @@ -1,16 +1,28 @@ // -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- +#include "Tracker.h" + // mission storage static const StorageAccess wp_storage(StorageManager::StorageMission); -static void init_tracker() +static void mavlink_snoop_static(const mavlink_message_t* msg) +{ + return tracker.mavlink_snoop(msg); +} + +static void mavlink_delay_cb_static() +{ + tracker.mavlink_delay_cb(); +} + +void Tracker::init_tracker() { // initialise console serial port serial_manager.init_console(); - cliSerial->printf_P(PSTR("\n\nInit " THISFIRMWARE - "\n\nFree RAM: %u\n"), - hal.util->available_memory()); + hal.console->printf_P(PSTR("\n\nInit " THISFIRMWARE + "\n\nFree RAM: %u\n"), + hal.util->available_memory()); // Check the EEPROM format version before loading any parameters from EEPROM load_parameters(); @@ -25,11 +37,11 @@ static void init_tracker() // init the GCS and start snooping for vehicle data gcs[0].setup_uart(serial_manager, AP_SerialManager::SerialProtocol_Console, 0); - gcs[0].set_snoop(mavlink_snoop); + gcs[0].set_snoop(mavlink_snoop_static); // Register mavlink_delay_cb, which will run anytime you have // more than 5ms remaining in your call to hal.scheduler->delay - hal.scheduler->register_delay_callback(mavlink_delay_cb, 5); + hal.scheduler->register_delay_callback(mavlink_delay_cb_static, 5); // we start by assuming USB connected, as we initialed the serial // port with SERIAL0_BAUD. check_usb_mux() fixes this if need be. @@ -38,25 +50,25 @@ static void init_tracker() // setup serial port for telem1 and start snooping for vehicle data gcs[1].setup_uart(serial_manager, AP_SerialManager::SerialProtocol_MAVLink, 0); - gcs[1].set_snoop(mavlink_snoop); + gcs[1].set_snoop(mavlink_snoop_static); #if MAVLINK_COMM_NUM_BUFFERS > 2 // setup serial port for telem2 and start snooping for vehicle data gcs[2].setup_uart(serial_manager, AP_SerialManager::SerialProtocol_MAVLink, 1); - gcs[2].set_snoop(mavlink_snoop); + gcs[2].set_snoop(mavlink_snoop_static); #endif #if MAVLINK_COMM_NUM_BUFFERS > 3 // setup serial port for fourth telemetry port (not used by default) and start snooping for vehicle data gcs[3].setup_uart(serial_manager, AP_SerialManager::SerialProtocol_MAVLink, 2); - gcs[3].set_snoop(mavlink_snoop); + gcs[3].set_snoop(mavlink_snoop_static); #endif mavlink_system.sysid = g.sysid_this_mav; if (g.compass_enabled==true) { if (!compass.init() || !compass.read()) { - cliSerial->println_P(PSTR("Compass initialisation failed!")); + hal.console->println_P(PSTR("Compass initialisation failed!")); g.compass_enabled = false; } else { ahrs.set_compass(&compass); @@ -107,7 +119,7 @@ static void init_tracker() // updates the status of the notify objects // should be called at 50hz -static void update_notify() +void Tracker::update_notify() { notify.update(); } @@ -115,7 +127,7 @@ static void update_notify() /* fetch HOME from EEPROM */ -static bool get_home_eeprom(struct Location &loc) +bool Tracker::get_home_eeprom(struct Location &loc) { // Find out proper location in memory by using the start_byte position + the index // -------------------------------------------------------------------------------- @@ -132,7 +144,7 @@ static bool get_home_eeprom(struct Location &loc) return true; } -static void set_home_eeprom(struct Location temp) +void Tracker::set_home_eeprom(struct Location temp) { wp_storage.write_byte(0, temp.options); wp_storage.write_uint32(1, temp.alt); @@ -143,19 +155,19 @@ static void set_home_eeprom(struct Location temp) g.command_total.set_and_save(1); // At most 1 entry for HOME } -static void set_home(struct Location temp) +void Tracker::set_home(struct Location temp) { set_home_eeprom(temp); current_loc = temp; } -static void arm_servos() +void Tracker::arm_servos() { channel_yaw.enable_out(); channel_pitch.enable_out(); } -static void disarm_servos() +void Tracker::disarm_servos() { channel_yaw.disable_out(); channel_pitch.disable_out(); @@ -164,7 +176,7 @@ static void disarm_servos() /* setup servos to trim value after initialising */ -static void prepare_servos() +void Tracker::prepare_servos() { start_time_ms = hal.scheduler->millis(); channel_yaw.radio_out = channel_yaw.radio_trim; @@ -173,7 +185,7 @@ static void prepare_servos() channel_pitch.output(); } -static void set_mode(enum ControlMode mode) +void Tracker::set_mode(enum ControlMode mode) { if(control_mode == mode) { // don't switch modes if we are already in the correct mode. @@ -199,7 +211,7 @@ static void set_mode(enum ControlMode mode) /* set_mode() wrapper for MAVLink SET_MODE */ -static bool mavlink_set_mode(uint8_t mode) +bool Tracker::mavlink_set_mode(uint8_t mode) { switch (mode) { case AUTO: @@ -213,7 +225,7 @@ static bool mavlink_set_mode(uint8_t mode) return false; } -static void check_usb_mux(void) +void Tracker::check_usb_mux(void) { bool usb_check = hal.gpio->usb_connected(); if (usb_check == usb_connected) { diff --git a/AntennaTracker/tracking.cpp b/AntennaTracker/tracking.cpp index 7636683681..882f3a4b65 100644 --- a/AntennaTracker/tracking.cpp +++ b/AntennaTracker/tracking.cpp @@ -1,10 +1,12 @@ /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- +#include "Tracker.h" + /** update_vehicle_position_estimate - updates estimate of vehicle positions should be called at 50hz */ -static void update_vehicle_pos_estimate() +void Tracker::update_vehicle_pos_estimate() { // calculate time since last actual position update float dt = (hal.scheduler->micros() - vehicle.last_update_us) * 1.0e-6f; @@ -26,7 +28,7 @@ static void update_vehicle_pos_estimate() update_tracker_position - updates antenna tracker position from GPS location should be called at 50hz */ -static void update_tracker_position() +void Tracker::update_tracker_position() { // update our position if we have at least a 2D fix // REVISIT: what if we lose lock during a mission and the antenna is moving? @@ -39,7 +41,7 @@ static void update_tracker_position() update_bearing_and_distance - updates bearing and distance to the vehicle should be called at 50hz */ -static void update_bearing_and_distance() +void Tracker::update_bearing_and_distance() { // exit immediately if we do not have a valid vehicle position if (!vehicle.location_valid) { @@ -65,7 +67,7 @@ static void update_bearing_and_distance() /** main antenna tracking code, called at 50Hz */ -static void update_tracking(void) +void Tracker::update_tracking(void) { // update vehicle position estimate update_vehicle_pos_estimate(); @@ -110,7 +112,7 @@ static void update_tracking(void) /** handle an updated position from the aircraft */ -static void tracking_update_position(const mavlink_global_position_int_t &msg) +void Tracker::tracking_update_position(const mavlink_global_position_int_t &msg) { vehicle.location.lat = msg.lat; vehicle.location.lng = msg.lon; @@ -125,7 +127,7 @@ static void tracking_update_position(const mavlink_global_position_int_t &msg) /** handle an updated pressure reading from the aircraft */ -static void tracking_update_pressure(const mavlink_scaled_pressure_t &msg) +void Tracker::tracking_update_pressure(const mavlink_scaled_pressure_t &msg) { float local_pressure = barometer.get_pressure(); float aircraft_pressure = msg.press_abs*100.0f; @@ -148,7 +150,7 @@ static void tracking_update_pressure(const mavlink_scaled_pressure_t &msg) /** handle a manual control message by using the data to command yaw and pitch */ -static void tracking_manual_control(const mavlink_manual_control_t &msg) +void Tracker::tracking_manual_control(const mavlink_manual_control_t &msg) { nav_status.bearing = msg.x; nav_status.pitch = msg.y; @@ -161,7 +163,7 @@ static void tracking_manual_control(const mavlink_manual_control_t &msg) /** update_armed_disarmed - set armed LED if we have received a position update within the last 5 seconds */ -static void update_armed_disarmed() +void Tracker::update_armed_disarmed() { if (vehicle.last_update_ms != 0 && (hal.scheduler->millis() - vehicle.last_update_ms) < TRACKING_TIMEOUT_MS) { AP_Notify::flags.armed = true;