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

AntennaTracker: complete conversion to .cpp files

This commit is contained in:
Andrew Tridgell 2015-06-01 15:04:25 +10:00
parent 1660b027ca
commit f8adea2ddd
14 changed files with 496 additions and 337 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

262
AntennaTracker/AntennaTracker.cpp
View File

@ -20,192 +20,9 @@
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
////////////////////////////////////////////////////////////////////////////////
// Header includes
////////////////////////////////////////////////////////////////////////////////
#include "Tracker.h"
#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 <StorageManager.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_SerialManager.h> // Serial manager library
#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_Mission.h>
#include <AP_Terrain.h>
#include <AP_Rally.h>
#include <AP_Notify.h> // Notify library
#include <AP_BattMonitor.h> // Battery monitor library
#include <AP_Airspeed.h>
#include <RC_Channel.h>
#include <AP_BoardConfig.h>
#include <AP_OpticalFlow.h>
#include <AP_RangeFinder.h>
// Configuration
#include "config.h"
// Local modules
#include "defines.h"
#include "Parameters.h"
#include "GCS.h"
#include <AP_HAL_AVR.h>
#include <AP_HAL_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;
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();

122
AntennaTracker/GCS_Mavlink.cpp
View File

@ -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; i<num_gcs; i++) {
if (gcs[i].initialised) {
@ -902,7 +902,7 @@ static void gcs_send_message(enum ap_message id)
/*
* send data streams in the given rate range on both links
*/
static void gcs_data_stream_send(void)
void Tracker::gcs_data_stream_send(void)
{
for (uint8_t i=0; i<num_gcs; i++) {
if (gcs[i].initialised) {
@ -914,7 +914,7 @@ static void gcs_data_stream_send(void)
/*
* look for incoming commands on the GCS links
*/
static void gcs_update(void)
void Tracker::gcs_update(void)
{
for (uint8_t i=0; i<num_gcs; i++) {
if (gcs[i].initialised) {
@ -923,7 +923,7 @@ static void gcs_update(void)
}
}
static void gcs_send_text_P(gcs_severity severity, const prog_char_t *str)
void Tracker::gcs_send_text_P(gcs_severity severity, const prog_char_t *str)
{
for (uint8_t i=0; i<num_gcs; i++) {
if (gcs[i].initialised) {
@ -940,7 +940,7 @@ static void gcs_send_text_P(gcs_severity severity, const prog_char_t *str)
* only one fits in the queue, so if you send more than one before the
* last one gets into the serial buffer then the old one will be lost
*/
void gcs_send_text_fmt(const prog_char_t *fmt, ...)
void Tracker::gcs_send_text_fmt(const prog_char_t *fmt, ...)
{
va_list arg_list;
gcs[0].pending_status.severity = (uint8_t)SEVERITY_LOW;
@ -963,7 +963,7 @@ void gcs_send_text_fmt(const prog_char_t *fmt, ...)
/**
retry any deferred messages
*/
static void gcs_retry_deferred(void)
void Tracker::gcs_retry_deferred(void)
{
gcs_send_message(MSG_RETRY_DEFERRED);
}

22
AntennaTracker/Parameters.cpp
View File

@ -1,17 +1,19 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#include "Tracker.h"
/*
* AntennaTracker parameter definitions
*
*/
#define GSCALAR(v, name, def) { g.v.vtype, name, Parameters::k_param_ ## v, &g.v, {def_value : def} }
#define ASCALAR(v, name, def) { aparm.v.vtype, name, Parameters::k_param_ ## v, &aparm.v, {def_value : def} }
#define GGROUP(v, name, class) { AP_PARAM_GROUP, name, Parameters::k_param_ ## v, &g.v, {group_info : class::var_info} }
#define GOBJECT(v, name, class) { AP_PARAM_GROUP, name, Parameters::k_param_ ## v, &v, {group_info : class::var_info} }
#define GOBJECTN(v, pname, name, class) { AP_PARAM_GROUP, name, Parameters::k_param_ ## pname, &v, {group_info : class::var_info} }
#define GSCALAR(v, name, def) { tracker.g.v.vtype, name, Parameters::k_param_ ## v, &tracker.g.v, {def_value : def} }
#define ASCALAR(v, name, def) { tracker.aparm.v.vtype, name, Parameters::k_param_ ## v, (const void *)&tracker.aparm.v, {def_value : def} }
#define GGROUP(v, name, class) { AP_PARAM_GROUP, name, Parameters::k_param_ ## v, &tracker.g.v, {group_info : class::var_info} }
#define GOBJECT(v, name, class) { AP_PARAM_GROUP, name, Parameters::k_param_ ## v, (const void *)&tracker.v, {group_info : class::var_info} }
#define GOBJECTN(v, pname, name, class) { AP_PARAM_GROUP, name, Parameters::k_param_ ## pname, (const void *)&tracker.v, {group_info : class::var_info} }
const AP_Param::Info var_info[] PROGMEM = {
const AP_Param::Info Tracker::var_info[] PROGMEM = {
GSCALAR(format_version, "FORMAT_VERSION", 0),
GSCALAR(software_type, "SYSID_SW_TYPE", Parameters::k_software_type),
@ -280,22 +282,22 @@ const AP_Param::Info var_info[] PROGMEM = {
};
static void load_parameters(void)
void Tracker::load_parameters(void)
{
if (!g.format_version.load() ||
g.format_version != Parameters::k_format_version) {
// erase all parameters
cliSerial->printf_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));
}
}

253
AntennaTracker/Tracker.h Normal file
View File

@ -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 <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 <StorageManager.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_SerialManager.h> // Serial manager library
#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_Mission.h>
#include <AP_Terrain.h>
#include <AP_Rally.h>
#include <AP_Notify.h> // Notify library
#include <AP_BattMonitor.h> // Battery monitor library
#include <AP_Airspeed.h>
#include <RC_Channel.h>
#include <AP_BoardConfig.h>
#include <AP_OpticalFlow.h>
#include <AP_RangeFinder.h>
// Configuration
#include "config.h"
#include "defines.h"
#include "Parameters.h"
#include "GCS.h"
#include <AP_HAL_AVR.h>
#include <AP_HAL_SITL.h>
#include <AP_HAL_PX4.h>
#include <AP_HAL_FLYMAPLE.h>
#include <AP_HAL_Linux.h>
#include <AP_HAL_Empty.h>
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;

4
AntennaTracker/control_auto.cpp
View File

@ -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) {

4
AntennaTracker/control_manual.cpp
View File

@ -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);

4
AntennaTracker/control_scan.cpp
View File

@ -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;

4
AntennaTracker/control_servo_test.cpp
View File

@ -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--;

40
AntennaTracker/make.inc Normal file
View File

@ -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

4
AntennaTracker/radio.cpp
View File

@ -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));

16
AntennaTracker/sensors.cpp
View File

@ -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();

28
AntennaTracker/servos.cpp
View File

@ -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);

52
AntennaTracker/system.cpp
View File

@ -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) {

18
AntennaTracker/tracking.cpp
View File

@ -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;