mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-20 07:38:28 -04:00
8f71af605b
AntennaTracker has not been maintained as other parts of the system and libraries have been modernised. This patch at least gets it to the stage where it compiles and runs in SITL. Also added Tools/autotest/sim_antennatracker.sh to run in SITL
275 lines
7.9 KiB
Plaintext
275 lines
7.9 KiB
Plaintext
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
|
|
|
#define THISFIRMWARE "AntennaTracker V0.1"
|
|
/*
|
|
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 <http://www.gnu.org/licenses/>.
|
|
*/
|
|
|
|
////////////////////////////////////////////////////////////////////////////////
|
|
// Header includes
|
|
////////////////////////////////////////////////////////////////////////////////
|
|
|
|
#include <math.h>
|
|
#include <stdarg.h>
|
|
#include <stdio.h>
|
|
|
|
#include <AP_Common.h>
|
|
#include <AP_Progmem.h>
|
|
#include <AP_HAL.h>
|
|
#include <AP_Param.h>
|
|
#include <AP_GPS.h> // ArduPilot GPS library
|
|
#include <AP_Baro.h> // ArduPilot barometer library
|
|
#include <AP_Compass.h> // ArduPilot Mega Magnetometer Library
|
|
#include <AP_Math.h> // ArduPilot Mega Vector/Matrix math Library
|
|
#include <AP_ADC.h> // ArduPilot Mega Analog to Digital Converter Library
|
|
#include <AP_ADC_AnalogSource.h>
|
|
#include <AP_InertialSensor.h> // Inertial Sensor Library
|
|
#include <AP_AHRS.h> // ArduPilot Mega DCM Library
|
|
#include <Filter.h> // Filter library
|
|
#include <AP_Buffer.h> // APM FIFO Buffer
|
|
#include <memcheck.h>
|
|
|
|
#include <GCS_MAVLink.h> // MAVLink GCS definitions
|
|
#include <AP_Declination.h> // ArduPilot Mega Declination Helper Library
|
|
#include <DataFlash.h>
|
|
#include <SITL.h>
|
|
#include <PID.h>
|
|
#include <AP_Scheduler.h> // main loop scheduler
|
|
#include <AP_NavEKF.h>
|
|
|
|
#include <AP_Vehicle.h>
|
|
#include <AP_Notify.h> // Notify library
|
|
#include <AP_BattMonitor.h> // Battery monitor library
|
|
#include <AP_Airspeed.h>
|
|
#include <RC_Channel.h>
|
|
|
|
// Configuration
|
|
#include "config.h"
|
|
|
|
// Local modules
|
|
#include "defines.h"
|
|
|
|
#include "Parameters.h"
|
|
#include "GCS.h"
|
|
|
|
#include <AP_HAL_AVR.h>
|
|
#include <AP_HAL_AVR_SITL.h>
|
|
#include <AP_HAL_PX4.h>
|
|
#include <AP_HAL_FLYMAPLE.h>
|
|
#include <AP_HAL_Linux.h>
|
|
#include <AP_HAL_Empty.h>
|
|
|
|
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;
|
|
|
|
// tracking status for MAVLink
|
|
static struct {
|
|
float bearing;
|
|
float distance;
|
|
float pitch;
|
|
} nav_status;
|
|
|
|
|
|
////////////////////////////////////////////////////////////////////////////////
|
|
// prototypes
|
|
void gcs_send_text_fmt(const prog_char_t *fmt, ...);
|
|
|
|
|
|
////////////////////////////////////////////////////////////////////////////////
|
|
// Sensors
|
|
////////////////////////////////////////////////////////////////////////////////
|
|
// All GPS access should be through this pointer.
|
|
static GPS *g_gps;
|
|
|
|
#if CONFIG_BARO == AP_BARO_BMP085
|
|
static AP_Baro_BMP085 barometer;
|
|
#elif CONFIG_BARO == AP_BARO_PX4
|
|
static AP_Baro_PX4 barometer;
|
|
#elif CONFIG_BARO == AP_BARO_HIL
|
|
static AP_Baro_HIL barometer;
|
|
#elif CONFIG_BARO == AP_BARO_MS5611
|
|
#if CONFIG_MS5611_SERIAL == AP_BARO_MS5611_SPI
|
|
static AP_Baro_MS5611 barometer(&AP_Baro_MS5611::spi);
|
|
#elif CONFIG_MS5611_SERIAL == AP_BARO_MS5611_I2C
|
|
static AP_Baro_MS5611 barometer(&AP_Baro_MS5611::i2c);
|
|
#else
|
|
#error Unrecognized CONFIG_MS5611_SERIAL setting.
|
|
#endif
|
|
#else
|
|
#error Unrecognized CONFIG_BARO setting
|
|
#endif
|
|
|
|
#if CONFIG_COMPASS == AP_COMPASS_PX4
|
|
static AP_Compass_PX4 compass;
|
|
#elif CONFIG_COMPASS == AP_COMPASS_HMC5843
|
|
static AP_Compass_HMC5843 compass;
|
|
#elif CONFIG_COMPASS == AP_COMPASS_HIL
|
|
static AP_Compass_HIL compass;
|
|
#else
|
|
#error Unrecognized CONFIG_COMPASS setting
|
|
#endif
|
|
|
|
// GPS selection
|
|
AP_GPS_Auto g_gps_driver(&g_gps);
|
|
|
|
#if CONFIG_INS_TYPE == CONFIG_INS_MPU6000
|
|
AP_InertialSensor_MPU6000 ins;
|
|
#elif CONFIG_INS_TYPE == CONFIG_INS_PX4
|
|
AP_InertialSensor_PX4 ins;
|
|
#elif CONFIG_INS_TYPE == CONFIG_INS_HIL
|
|
AP_InertialSensor_HIL ins;
|
|
#elif CONFIG_INS_TYPE == CONFIG_INS_OILPAN
|
|
AP_InertialSensor_Oilpan ins( &apm1_adc );
|
|
#elif CONFIG_INS_TYPE == CONFIG_INS_FLYMAPLE
|
|
AP_InertialSensor_Flymaple ins;
|
|
#elif CONFIG_INS_TYPE == CONFIG_INS_L3G4200D
|
|
AP_InertialSensor_L3G4200D ins;
|
|
#else
|
|
#error Unrecognised CONFIG_INS_TYPE setting.
|
|
#endif // CONFIG_INS_TYPE
|
|
|
|
// Inertial Navigation EKF
|
|
#if AP_AHRS_NAVEKF_AVAILABLE
|
|
AP_AHRS_NavEKF ahrs(ins, barometer, g_gps);
|
|
#else
|
|
AP_AHRS_DCM ahrs(ins, barometer, g_gps);
|
|
#endif
|
|
|
|
#if CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL
|
|
SITL sitl;
|
|
#endif
|
|
|
|
/**
|
|
antenna control channels
|
|
*/
|
|
static RC_Channel channel_yaw(CH_1);
|
|
static RC_Channel channel_pitch(CH_2);
|
|
|
|
////////////////////////////////////////////////////////////////////////////////
|
|
// GCS selection
|
|
////////////////////////////////////////////////////////////////////////////////
|
|
static GCS_MAVLINK gcs0;
|
|
static GCS_MAVLINK gcs3;
|
|
|
|
/*
|
|
scheduler table - all regular tasks apart from the fast_loop()
|
|
should be listed here, along with how often they should be called
|
|
(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 },
|
|
{ update_tracking, 1, 1000 },
|
|
{ update_GPS, 5, 4000 },
|
|
{ update_compass, 5, 1500 },
|
|
{ update_barometer, 5, 1500 },
|
|
{ update_tracking, 1, 1000 },
|
|
{ gcs_update, 1, 1700 },
|
|
{ gcs_data_stream_send, 1, 3000 },
|
|
{ compass_accumulate, 1, 1500 },
|
|
{ barometer_accumulate, 1, 900 },
|
|
{ update_notify, 1, 100 },
|
|
{ gcs_retry_deferred, 1, 1000 },
|
|
{ one_second_loop, 50, 3900 }
|
|
};
|
|
|
|
// setup the var_info table
|
|
AP_Param param_loader(var_info, EEPROM_MAX_ADDR);
|
|
|
|
/**
|
|
setup the sketch - called once on startup
|
|
*/
|
|
void setup()
|
|
{
|
|
cliSerial = hal.console;
|
|
|
|
// load the default values of variables listed in var_info[]
|
|
AP_Param::setup_sketch_defaults();
|
|
|
|
// arduplane does not use arming nor pre-arm checks
|
|
AP_Notify::flags.armed = true;
|
|
AP_Notify::flags.pre_arm_check = true;
|
|
AP_Notify::flags.failsafe_battery = false;
|
|
|
|
notify.init(false);
|
|
init_tracker();
|
|
|
|
// initialise the main loop scheduler
|
|
scheduler.init(&scheduler_tasks[0], sizeof(scheduler_tasks)/sizeof(scheduler_tasks[0]));
|
|
}
|
|
|
|
/**
|
|
loop() is called continuously
|
|
*/
|
|
void loop()
|
|
{
|
|
// wait for an INS sample
|
|
if (!ins.wait_for_sample(1000)) {
|
|
return;
|
|
}
|
|
|
|
// tell the scheduler one tick has passed
|
|
scheduler.tick();
|
|
|
|
scheduler.run(19900UL);
|
|
}
|
|
|
|
static void one_second_loop()
|
|
{
|
|
// send a heartbeat
|
|
gcs_send_message(MSG_HEARTBEAT);
|
|
|
|
// make it possible to change orientation at runtime
|
|
ahrs.set_orientation();
|
|
|
|
// sync MAVLink system ID
|
|
mavlink_system.sysid = g.sysid_this_mav;
|
|
|
|
static uint8_t counter;
|
|
counter++;
|
|
|
|
if (counter >= 60) {
|
|
if(g.compass_enabled) {
|
|
compass.save_offsets();
|
|
}
|
|
counter = 0;
|
|
}
|
|
}
|
|
|
|
AP_HAL_MAIN();
|