mirror of https://github.com/ArduPilot/ardupilot
AntennaTracker: first cut at antenna tracking sketch
This commit is contained in:
Andrew Tridgell
parent
de96ad9445
commit
871a606ce1
|
|
@ -0,0 +1,5 @@
|
|||
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
||||
|
||||
// This file is just a placeholder for your configuration file. If
|
||||
// you wish to change any of the setup parameters from their default
|
||||
// values, place the appropriate #define statements here.
|
||||
|
|
@ -0,0 +1,271 @@
|
|||
/// -*- 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_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
|
||||
|
||||
AP_AHRS_DCM ahrs(&ins, g_gps);
|
||||
|
||||
#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 },
|
||||
{ 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()
|
||||
{
|
||||
// this needs to be the first call, as it fills memory with
|
||||
// sentinel values
|
||||
memcheck_init();
|
||||
|
||||
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();
|
||||
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();
|
||||
|
|
@ -0,0 +1,187 @@
|
|||
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
||||
|
||||
/// @file GCS.h
|
||||
/// @brief Interface definition for the various Ground Control System
|
||||
// protocols.
|
||||
|
||||
#ifndef __GCS_H
|
||||
#define __GCS_H
|
||||
|
||||
#include <AP_HAL.h>
|
||||
#include <AP_Common.h>
|
||||
#include <GPS.h>
|
||||
#include <stdint.h>
|
||||
|
||||
///
|
||||
/// @class GCS
|
||||
/// @brief Class describing the interface between the APM code
|
||||
/// proper and the GCS implementation.
|
||||
///
|
||||
/// GCS' are currently implemented inside the sketch and as such have
|
||||
/// access to all global state. The sketch should not, however, call GCS
|
||||
/// internal functions - all calls to the GCS should be routed through
|
||||
/// this interface (or functions explicitly exposed by a subclass).
|
||||
///
|
||||
class GCS_Class
|
||||
{
|
||||
public:
|
||||
|
||||
/// Startup initialisation.
|
||||
///
|
||||
/// This routine performs any one-off initialisation required before
|
||||
/// GCS messages are exchanged.
|
||||
///
|
||||
/// @note The stream is expected to be set up and configured for the
|
||||
/// correct bitrate before ::init is called.
|
||||
///
|
||||
/// @note The stream is currently BetterStream so that we can use the _P
|
||||
/// methods; this may change if Arduino adds them to Print.
|
||||
///
|
||||
/// @param port The stream over which messages are exchanged.
|
||||
///
|
||||
void init(AP_HAL::UARTDriver *port) {
|
||||
_port = port;
|
||||
initialised = true;
|
||||
}
|
||||
|
||||
/// Update GCS state.
|
||||
///
|
||||
/// This may involve checking for received bytes on the stream,
|
||||
/// or sending additional periodic messages.
|
||||
void update(void) {
|
||||
}
|
||||
|
||||
/// Send a message with a single numeric parameter.
|
||||
///
|
||||
/// This may be a standalone message, or the GCS driver may
|
||||
/// have its own way of locating additional parameters to send.
|
||||
///
|
||||
/// @param id ID of the message to send.
|
||||
/// @param param Explicit message parameter.
|
||||
///
|
||||
void send_message(enum ap_message id) {
|
||||
}
|
||||
|
||||
/// Send a text message.
|
||||
///
|
||||
/// @param severity A value describing the importance of the message.
|
||||
/// @param str The text to be sent.
|
||||
///
|
||||
void send_text(gcs_severity severity, const char *str) {
|
||||
}
|
||||
|
||||
/// Send a text message with a PSTR()
|
||||
///
|
||||
/// @param severity A value describing the importance of the message.
|
||||
/// @param str The text to be sent.
|
||||
///
|
||||
void send_text_P(gcs_severity severity, const prog_char_t *str) {
|
||||
}
|
||||
|
||||
// send streams which match frequency range
|
||||
void data_stream_send(void);
|
||||
|
||||
// set to true if this GCS link is active
|
||||
bool initialised;
|
||||
|
||||
protected:
|
||||
/// The stream we are communicating over
|
||||
AP_HAL::UARTDriver *_port;
|
||||
};
|
||||
|
||||
//
|
||||
// GCS class definitions.
|
||||
//
|
||||
// These are here so that we can declare the GCS object early in the sketch
|
||||
// and then reference it statically rather than via a pointer.
|
||||
//
|
||||
|
||||
///
|
||||
/// @class GCS_MAVLINK
|
||||
/// @brief The mavlink protocol for qgroundcontrol
|
||||
///
|
||||
class GCS_MAVLINK : public GCS_Class
|
||||
{
|
||||
public:
|
||||
GCS_MAVLINK();
|
||||
void update(void);
|
||||
void init(AP_HAL::UARTDriver *port);
|
||||
void send_message(enum ap_message id);
|
||||
void send_text(gcs_severity severity, const char *str);
|
||||
void send_text_P(gcs_severity severity, const prog_char_t *str);
|
||||
void data_stream_send(void);
|
||||
void queued_param_send();
|
||||
|
||||
static const struct AP_Param::GroupInfo var_info[];
|
||||
|
||||
// NOTE! The streams enum below and the
|
||||
// set of AP_Int16 stream rates _must_ be
|
||||
// kept in the same order
|
||||
enum streams {STREAM_RAW_SENSORS,
|
||||
STREAM_EXTENDED_STATUS,
|
||||
STREAM_RC_CHANNELS,
|
||||
STREAM_RAW_CONTROLLER,
|
||||
STREAM_POSITION,
|
||||
STREAM_EXTRA1,
|
||||
STREAM_EXTRA2,
|
||||
STREAM_EXTRA3,
|
||||
STREAM_PARAMS,
|
||||
NUM_STREAMS};
|
||||
|
||||
// see if we should send a stream now. Called at 50Hz
|
||||
bool stream_trigger(enum streams stream_num);
|
||||
|
||||
// this costs us 51 bytes per instance, but means that low priority
|
||||
// messages don't block the CPU
|
||||
mavlink_statustext_t pending_status;
|
||||
|
||||
// call to reset the timeout window for entering the cli
|
||||
void reset_cli_timeout();
|
||||
private:
|
||||
void handleMessage(mavlink_message_t * msg);
|
||||
|
||||
/// Perform queued sending operations
|
||||
///
|
||||
AP_Param * _queued_parameter; ///< next parameter to
|
||||
// be sent in queue
|
||||
enum ap_var_type _queued_parameter_type; ///< type of the next
|
||||
// parameter
|
||||
AP_Param::ParamToken _queued_parameter_token; ///AP_Param token for
|
||||
// next() call
|
||||
uint16_t _queued_parameter_index; ///< next queued
|
||||
// parameter's index
|
||||
uint16_t _queued_parameter_count; ///< saved count of
|
||||
// parameters for
|
||||
// queued send
|
||||
uint32_t _queued_parameter_send_time_ms;
|
||||
|
||||
/// Count the number of reportable parameters.
|
||||
///
|
||||
/// Not all parameters can be reported via MAVlink. We count the number
|
||||
// that are
|
||||
/// so that we can report to a GCS the number of parameters it should
|
||||
// expect when it
|
||||
/// requests the full set.
|
||||
///
|
||||
/// @return The number of reportable parameters.
|
||||
///
|
||||
uint16_t _count_parameters(); ///< count reportable
|
||||
// parameters
|
||||
|
||||
uint16_t _parameter_count; ///< cache of reportable
|
||||
// parameters
|
||||
|
||||
mavlink_channel_t chan;
|
||||
uint16_t packet_drops;
|
||||
|
||||
// saveable rate of each stream
|
||||
AP_Int16 streamRates[NUM_STREAMS];
|
||||
|
||||
// number of 50Hz ticks until we next send this stream
|
||||
uint8_t stream_ticks[NUM_STREAMS];
|
||||
|
||||
// number of extra ticks to add to slow things down for the radio
|
||||
uint8_t stream_slowdown;
|
||||
};
|
||||
|
||||
#endif // __GCS_H
|
||||
|
|
@ -0,0 +1,981 @@
|
|||
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
||||
|
||||
// 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)
|
||||
|
||||
// use this to prevent recursion during sensor init
|
||||
static bool in_mavlink_delay;
|
||||
|
||||
// true when we have received at least 1 MAVLink packet
|
||||
static bool mavlink_active;
|
||||
|
||||
// check if a message will fit in the payload space available
|
||||
#define CHECK_PAYLOAD_SIZE(id) if (payload_space < MAVLINK_MSG_ID_ ## id ## _LEN) return false
|
||||
|
||||
/*
|
||||
* !!NOTE!!
|
||||
*
|
||||
* the use of NOINLINE separate functions for each message type avoids
|
||||
* a compiler bug in gcc that would cause it to use far more stack
|
||||
* space than is needed. Without the NOINLINE we use the sum of the
|
||||
* stack needed for each message type. Please be careful to follow the
|
||||
* pattern below when adding any new messages
|
||||
*/
|
||||
|
||||
static NOINLINE void send_heartbeat(mavlink_channel_t chan)
|
||||
{
|
||||
mavlink_msg_heartbeat_send(
|
||||
chan,
|
||||
MAV_TYPE_ANTENNA_TRACKER,
|
||||
MAV_AUTOPILOT_ARDUPILOTMEGA,
|
||||
0,
|
||||
0,
|
||||
0);
|
||||
}
|
||||
|
||||
static NOINLINE void send_attitude(mavlink_channel_t chan)
|
||||
{
|
||||
Vector3f omega = ahrs.get_gyro();
|
||||
mavlink_msg_attitude_send(
|
||||
chan,
|
||||
hal.scheduler->millis(),
|
||||
ahrs.roll,
|
||||
ahrs.pitch,
|
||||
ahrs.yaw,
|
||||
omega.x,
|
||||
omega.y,
|
||||
omega.z);
|
||||
}
|
||||
|
||||
|
||||
static void NOINLINE send_location(mavlink_channel_t chan)
|
||||
{
|
||||
uint32_t fix_time;
|
||||
if (g_gps->status() >= GPS::GPS_OK_FIX_2D) {
|
||||
fix_time = g_gps->last_fix_time;
|
||||
} else {
|
||||
fix_time = hal.scheduler->millis();
|
||||
}
|
||||
|
||||
Location loc;
|
||||
ahrs.get_position(loc);
|
||||
|
||||
mavlink_msg_global_position_int_send(
|
||||
chan,
|
||||
fix_time,
|
||||
loc.lat, // in 1E7 degrees
|
||||
loc.lng, // in 1E7 degrees
|
||||
g_gps->altitude_cm * 10, // millimeters above sea level
|
||||
0,
|
||||
g_gps->velocity_north() * 100, // X speed cm/s (+ve North)
|
||||
g_gps->velocity_east() * 100, // Y speed cm/s (+ve East)
|
||||
g_gps->velocity_down() * -100, // Z speed cm/s (+ve up)
|
||||
ahrs.yaw_sensor);
|
||||
}
|
||||
|
||||
static void NOINLINE send_gps_raw(mavlink_channel_t chan)
|
||||
{
|
||||
mavlink_msg_gps_raw_int_send(
|
||||
chan,
|
||||
g_gps->last_fix_time*(uint64_t)1000,
|
||||
g_gps->status(),
|
||||
g_gps->latitude, // in 1E7 degrees
|
||||
g_gps->longitude, // in 1E7 degrees
|
||||
g_gps->altitude_cm * 10, // in mm
|
||||
g_gps->hdop,
|
||||
65535,
|
||||
g_gps->ground_speed_cm, // cm/s
|
||||
g_gps->ground_course_cd, // 1/100 degrees,
|
||||
g_gps->num_sats);
|
||||
}
|
||||
|
||||
static void NOINLINE send_radio_out(mavlink_channel_t chan)
|
||||
{
|
||||
mavlink_msg_servo_output_raw_send(
|
||||
chan,
|
||||
hal.scheduler->micros(),
|
||||
0, // port
|
||||
hal.rcout->read(0),
|
||||
hal.rcout->read(1),
|
||||
hal.rcout->read(2),
|
||||
hal.rcout->read(3),
|
||||
hal.rcout->read(4),
|
||||
hal.rcout->read(5),
|
||||
hal.rcout->read(6),
|
||||
hal.rcout->read(7));
|
||||
}
|
||||
|
||||
static void NOINLINE send_raw_imu1(mavlink_channel_t chan)
|
||||
{
|
||||
Vector3f accel = ins.get_accel();
|
||||
Vector3f gyro = ins.get_gyro();
|
||||
|
||||
mavlink_msg_raw_imu_send(
|
||||
chan,
|
||||
hal.scheduler->micros(),
|
||||
accel.x * 1000.0 / GRAVITY_MSS,
|
||||
accel.y * 1000.0 / GRAVITY_MSS,
|
||||
accel.z * 1000.0 / GRAVITY_MSS,
|
||||
gyro.x * 1000.0,
|
||||
gyro.y * 1000.0,
|
||||
gyro.z * 1000.0,
|
||||
compass.mag_x,
|
||||
compass.mag_y,
|
||||
compass.mag_z);
|
||||
}
|
||||
|
||||
static void NOINLINE send_raw_imu2(mavlink_channel_t chan)
|
||||
{
|
||||
float pressure = barometer.get_pressure();
|
||||
mavlink_msg_scaled_pressure_send(
|
||||
chan,
|
||||
hal.scheduler->millis(),
|
||||
pressure*0.01f, // hectopascal
|
||||
(pressure - barometer.get_ground_pressure())*0.01f, // hectopascal
|
||||
barometer.get_temperature()*100); // 0.01 degrees C
|
||||
}
|
||||
|
||||
static void NOINLINE send_raw_imu3(mavlink_channel_t chan)
|
||||
{
|
||||
// run this message at a much lower rate - otherwise it
|
||||
// pointlessly wastes quite a lot of bandwidth
|
||||
static uint8_t counter;
|
||||
if (counter++ < 10) {
|
||||
return;
|
||||
}
|
||||
counter = 0;
|
||||
|
||||
Vector3f mag_offsets = compass.get_offsets();
|
||||
Vector3f accel_offsets = ins.get_accel_offsets();
|
||||
Vector3f gyro_offsets = ins.get_gyro_offsets();
|
||||
|
||||
mavlink_msg_sensor_offsets_send(chan,
|
||||
mag_offsets.x,
|
||||
mag_offsets.y,
|
||||
mag_offsets.z,
|
||||
compass.get_declination(),
|
||||
barometer.get_pressure(),
|
||||
barometer.get_temperature()*100,
|
||||
gyro_offsets.x,
|
||||
gyro_offsets.y,
|
||||
gyro_offsets.z,
|
||||
accel_offsets.x,
|
||||
accel_offsets.y,
|
||||
accel_offsets.z);
|
||||
}
|
||||
|
||||
static void NOINLINE send_ahrs(mavlink_channel_t chan)
|
||||
{
|
||||
Vector3f omega_I = ahrs.get_gyro_drift();
|
||||
mavlink_msg_ahrs_send(
|
||||
chan,
|
||||
omega_I.x,
|
||||
omega_I.y,
|
||||
omega_I.z,
|
||||
0,
|
||||
0,
|
||||
ahrs.get_error_rp(),
|
||||
ahrs.get_error_yaw());
|
||||
}
|
||||
|
||||
static void NOINLINE send_hwstatus(mavlink_channel_t chan)
|
||||
{
|
||||
mavlink_msg_hwstatus_send(
|
||||
chan,
|
||||
0,
|
||||
hal.i2c->lockup_count());
|
||||
}
|
||||
|
||||
static void NOINLINE send_statustext(mavlink_channel_t chan)
|
||||
{
|
||||
mavlink_statustext_t *s = (chan == MAVLINK_COMM_0?&gcs0.pending_status:&gcs3.pending_status);
|
||||
mavlink_msg_statustext_send(
|
||||
chan,
|
||||
s->severity,
|
||||
s->text);
|
||||
}
|
||||
|
||||
static void NOINLINE send_nav_controller_output(mavlink_channel_t chan)
|
||||
{
|
||||
mavlink_msg_nav_controller_output_send(
|
||||
chan,
|
||||
0,
|
||||
nav_status.pitch,
|
||||
nav_status.bearing,
|
||||
nav_status.bearing,
|
||||
nav_status.distance,
|
||||
0,
|
||||
0,
|
||||
0);
|
||||
}
|
||||
|
||||
|
||||
// try to send a message, return false if it won't fit in the serial tx buffer
|
||||
static bool mavlink_try_send_message(mavlink_channel_t chan, enum ap_message id, uint16_t packet_drops)
|
||||
{
|
||||
int16_t payload_space = comm_get_txspace(chan) - MAVLINK_NUM_NON_PAYLOAD_BYTES;
|
||||
|
||||
switch (id) {
|
||||
case MSG_HEARTBEAT:
|
||||
CHECK_PAYLOAD_SIZE(HEARTBEAT);
|
||||
send_heartbeat(chan);
|
||||
return true;
|
||||
|
||||
case MSG_ATTITUDE:
|
||||
CHECK_PAYLOAD_SIZE(ATTITUDE);
|
||||
send_attitude(chan);
|
||||
break;
|
||||
|
||||
case MSG_LOCATION:
|
||||
CHECK_PAYLOAD_SIZE(GLOBAL_POSITION_INT);
|
||||
send_location(chan);
|
||||
break;
|
||||
|
||||
case MSG_NAV_CONTROLLER_OUTPUT:
|
||||
CHECK_PAYLOAD_SIZE(NAV_CONTROLLER_OUTPUT);
|
||||
send_nav_controller_output(chan);
|
||||
break;
|
||||
|
||||
case MSG_GPS_RAW:
|
||||
CHECK_PAYLOAD_SIZE(GPS_RAW_INT);
|
||||
send_gps_raw(chan);
|
||||
break;
|
||||
|
||||
case MSG_RADIO_OUT:
|
||||
CHECK_PAYLOAD_SIZE(SERVO_OUTPUT_RAW);
|
||||
send_radio_out(chan);
|
||||
break;
|
||||
|
||||
case MSG_RAW_IMU1:
|
||||
CHECK_PAYLOAD_SIZE(RAW_IMU);
|
||||
send_raw_imu1(chan);
|
||||
break;
|
||||
|
||||
case MSG_RAW_IMU2:
|
||||
CHECK_PAYLOAD_SIZE(SCALED_PRESSURE);
|
||||
send_raw_imu2(chan);
|
||||
break;
|
||||
|
||||
case MSG_RAW_IMU3:
|
||||
CHECK_PAYLOAD_SIZE(SENSOR_OFFSETS);
|
||||
send_raw_imu3(chan);
|
||||
break;
|
||||
|
||||
case MSG_NEXT_PARAM:
|
||||
CHECK_PAYLOAD_SIZE(PARAM_VALUE);
|
||||
if (chan == MAVLINK_COMM_0) {
|
||||
gcs0.queued_param_send();
|
||||
} else if (gcs3.initialised) {
|
||||
gcs3.queued_param_send();
|
||||
}
|
||||
break;
|
||||
|
||||
case MSG_STATUSTEXT:
|
||||
CHECK_PAYLOAD_SIZE(STATUSTEXT);
|
||||
send_statustext(chan);
|
||||
break;
|
||||
|
||||
case MSG_AHRS:
|
||||
CHECK_PAYLOAD_SIZE(AHRS);
|
||||
send_ahrs(chan);
|
||||
break;
|
||||
|
||||
case MSG_HWSTATUS:
|
||||
CHECK_PAYLOAD_SIZE(HWSTATUS);
|
||||
send_hwstatus(chan);
|
||||
break;
|
||||
|
||||
case MSG_SERVO_OUT:
|
||||
case MSG_EXTENDED_STATUS1:
|
||||
case MSG_EXTENDED_STATUS2:
|
||||
case MSG_RETRY_DEFERRED:
|
||||
break; // just here to prevent a warning
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
#define MAX_DEFERRED_MESSAGES MSG_RETRY_DEFERRED
|
||||
static struct mavlink_queue {
|
||||
enum ap_message deferred_messages[MAX_DEFERRED_MESSAGES];
|
||||
uint8_t next_deferred_message;
|
||||
uint8_t num_deferred_messages;
|
||||
} mavlink_queue[2];
|
||||
|
||||
// send a message using mavlink
|
||||
static void mavlink_send_message(mavlink_channel_t chan, enum ap_message id, uint16_t packet_drops)
|
||||
{
|
||||
uint8_t i, nextid;
|
||||
struct mavlink_queue *q = &mavlink_queue[(uint8_t)chan];
|
||||
|
||||
// see if we can send the deferred messages, if any
|
||||
while (q->num_deferred_messages != 0) {
|
||||
if (!mavlink_try_send_message(chan,
|
||||
q->deferred_messages[q->next_deferred_message],
|
||||
packet_drops)) {
|
||||
break;
|
||||
}
|
||||
q->next_deferred_message++;
|
||||
if (q->next_deferred_message == MAX_DEFERRED_MESSAGES) {
|
||||
q->next_deferred_message = 0;
|
||||
}
|
||||
q->num_deferred_messages--;
|
||||
}
|
||||
|
||||
if (id == MSG_RETRY_DEFERRED) {
|
||||
return;
|
||||
}
|
||||
|
||||
// this message id might already be deferred
|
||||
for (i=0, nextid = q->next_deferred_message; i < q->num_deferred_messages; i++) {
|
||||
if (q->deferred_messages[nextid] == id) {
|
||||
// its already deferred, discard
|
||||
return;
|
||||
}
|
||||
nextid++;
|
||||
if (nextid == MAX_DEFERRED_MESSAGES) {
|
||||
nextid = 0;
|
||||
}
|
||||
}
|
||||
|
||||
if (q->num_deferred_messages != 0 ||
|
||||
!mavlink_try_send_message(chan, id, packet_drops)) {
|
||||
// can't send it now, so defer it
|
||||
if (q->num_deferred_messages == MAX_DEFERRED_MESSAGES) {
|
||||
// the defer buffer is full, discard
|
||||
return;
|
||||
}
|
||||
nextid = q->next_deferred_message + q->num_deferred_messages;
|
||||
if (nextid >= MAX_DEFERRED_MESSAGES) {
|
||||
nextid -= MAX_DEFERRED_MESSAGES;
|
||||
}
|
||||
q->deferred_messages[nextid] = id;
|
||||
q->num_deferred_messages++;
|
||||
}
|
||||
}
|
||||
|
||||
void mavlink_send_text(mavlink_channel_t chan, gcs_severity severity, const char *str)
|
||||
{
|
||||
if (severity == SEVERITY_LOW) {
|
||||
// send via the deferred queuing system
|
||||
mavlink_statustext_t *s = (chan == MAVLINK_COMM_0?&gcs0.pending_status:&gcs3.pending_status);
|
||||
s->severity = (uint8_t)severity;
|
||||
strncpy((char *)s->text, str, sizeof(s->text));
|
||||
mavlink_send_message(chan, MSG_STATUSTEXT, 0);
|
||||
} else {
|
||||
// send immediately
|
||||
mavlink_msg_statustext_send(chan, severity, str);
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
default stream rates to 1Hz
|
||||
*/
|
||||
const AP_Param::GroupInfo GCS_MAVLINK::var_info[] PROGMEM = {
|
||||
// @Param: RAW_SENS
|
||||
// @DisplayName: Raw sensor stream rate
|
||||
// @Description: Raw sensor stream rate to ground station
|
||||
// @Units: Hz
|
||||
// @Range: 0 10
|
||||
// @Increment: 1
|
||||
// @User: Advanced
|
||||
AP_GROUPINFO("RAW_SENS", 0, GCS_MAVLINK, streamRates[0], 1),
|
||||
|
||||
// @Param: EXT_STAT
|
||||
// @DisplayName: Extended status stream rate to ground station
|
||||
// @Description: Extended status stream rate to ground station
|
||||
// @Units: Hz
|
||||
// @Range: 0 10
|
||||
// @Increment: 1
|
||||
// @User: Advanced
|
||||
AP_GROUPINFO("EXT_STAT", 1, GCS_MAVLINK, streamRates[1], 1),
|
||||
|
||||
// @Param: RC_CHAN
|
||||
// @DisplayName: RC Channel stream rate to ground station
|
||||
// @Description: RC Channel stream rate to ground station
|
||||
// @Units: Hz
|
||||
// @Range: 0 10
|
||||
// @Increment: 1
|
||||
// @User: Advanced
|
||||
AP_GROUPINFO("RC_CHAN", 2, GCS_MAVLINK, streamRates[2], 1),
|
||||
|
||||
// @Param: RAW_CTRL
|
||||
// @DisplayName: Raw Control stream rate to ground station
|
||||
// @Description: Raw Control stream rate to ground station
|
||||
// @Units: Hz
|
||||
// @Range: 0 10
|
||||
// @Increment: 1
|
||||
// @User: Advanced
|
||||
AP_GROUPINFO("RAW_CTRL", 3, GCS_MAVLINK, streamRates[3], 1),
|
||||
|
||||
// @Param: POSITION
|
||||
// @DisplayName: Position stream rate to ground station
|
||||
// @Description: Position stream rate to ground station
|
||||
// @Units: Hz
|
||||
// @Range: 0 10
|
||||
// @Increment: 1
|
||||
// @User: Advanced
|
||||
AP_GROUPINFO("POSITION", 4, GCS_MAVLINK, streamRates[4], 1),
|
||||
|
||||
// @Param: EXTRA1
|
||||
// @DisplayName: Extra data type 1 stream rate to ground station
|
||||
// @Description: Extra data type 1 stream rate to ground station
|
||||
// @Units: Hz
|
||||
// @Range: 0 10
|
||||
// @Increment: 1
|
||||
// @User: Advanced
|
||||
AP_GROUPINFO("EXTRA1", 5, GCS_MAVLINK, streamRates[5], 1),
|
||||
|
||||
// @Param: EXTRA2
|
||||
// @DisplayName: Extra data type 2 stream rate to ground station
|
||||
// @Description: Extra data type 2 stream rate to ground station
|
||||
// @Units: Hz
|
||||
// @Range: 0 10
|
||||
// @Increment: 1
|
||||
// @User: Advanced
|
||||
AP_GROUPINFO("EXTRA2", 6, GCS_MAVLINK, streamRates[6], 1),
|
||||
|
||||
// @Param: EXTRA3
|
||||
// @DisplayName: Extra data type 3 stream rate to ground station
|
||||
// @Description: Extra data type 3 stream rate to ground station
|
||||
// @Units: Hz
|
||||
// @Range: 0 10
|
||||
// @Increment: 1
|
||||
// @User: Advanced
|
||||
AP_GROUPINFO("EXTRA3", 7, GCS_MAVLINK, streamRates[7], 1),
|
||||
|
||||
// @Param: PARAMS
|
||||
// @DisplayName: Parameter stream rate to ground station
|
||||
// @Description: Parameter stream rate to ground station
|
||||
// @Units: Hz
|
||||
// @Range: 0 10
|
||||
// @Increment: 1
|
||||
// @User: Advanced
|
||||
AP_GROUPINFO("PARAMS", 8, GCS_MAVLINK, streamRates[8], 10),
|
||||
AP_GROUPEND
|
||||
};
|
||||
|
||||
|
||||
GCS_MAVLINK::GCS_MAVLINK() :
|
||||
packet_drops(0)
|
||||
{
|
||||
AP_Param::setup_object_defaults(this, var_info);
|
||||
}
|
||||
|
||||
void
|
||||
GCS_MAVLINK::init(AP_HAL::UARTDriver *port)
|
||||
{
|
||||
GCS_Class::init(port);
|
||||
if (port == (AP_HAL::BetterStream*)hal.uartA) {
|
||||
mavlink_comm_0_port = port;
|
||||
chan = MAVLINK_COMM_0;
|
||||
}else{
|
||||
mavlink_comm_1_port = port;
|
||||
chan = MAVLINK_COMM_1;
|
||||
}
|
||||
_queued_parameter = NULL;
|
||||
}
|
||||
|
||||
void
|
||||
GCS_MAVLINK::update(void)
|
||||
{
|
||||
// receive new packets
|
||||
mavlink_message_t msg;
|
||||
mavlink_status_t status;
|
||||
status.packet_rx_drop_count = 0;
|
||||
|
||||
// process received bytes
|
||||
uint16_t nbytes = comm_get_available(chan);
|
||||
for (uint16_t i=0; i<nbytes; i++)
|
||||
{
|
||||
uint8_t c = comm_receive_ch(chan);
|
||||
|
||||
// Try to get a new message
|
||||
if (mavlink_parse_char(chan, c, &msg, &status)) {
|
||||
// we exclude radio packets to make it possible to use the
|
||||
// CLI over the radio
|
||||
if (msg.msgid != MAVLINK_MSG_ID_RADIO && msg.msgid != MAVLINK_MSG_ID_RADIO_STATUS) {
|
||||
mavlink_active = true;
|
||||
}
|
||||
handleMessage(&msg);
|
||||
}
|
||||
}
|
||||
|
||||
// Update packet drops counter
|
||||
packet_drops += status.packet_rx_drop_count;
|
||||
}
|
||||
|
||||
// see if we should send a stream now. Called at 50Hz
|
||||
bool GCS_MAVLINK::stream_trigger(enum streams stream_num)
|
||||
{
|
||||
if (stream_num >= NUM_STREAMS) {
|
||||
return false;
|
||||
}
|
||||
float rate = (uint8_t)streamRates[stream_num].get();
|
||||
|
||||
// send at a much lower rate during parameter sends
|
||||
if (_queued_parameter != NULL) {
|
||||
rate *= 0.25;
|
||||
}
|
||||
|
||||
if (rate <= 0) {
|
||||
return false;
|
||||
}
|
||||
|
||||
if (stream_ticks[stream_num] == 0) {
|
||||
// we're triggering now, setup the next trigger point
|
||||
if (rate > 50) {
|
||||
rate = 50;
|
||||
}
|
||||
stream_ticks[stream_num] = (50 / rate) + stream_slowdown;
|
||||
return true;
|
||||
}
|
||||
|
||||
// count down at 50Hz
|
||||
stream_ticks[stream_num]--;
|
||||
return false;
|
||||
}
|
||||
|
||||
void
|
||||
GCS_MAVLINK::data_stream_send(void)
|
||||
{
|
||||
if (_queued_parameter != NULL) {
|
||||
if (streamRates[STREAM_PARAMS].get() <= 0) {
|
||||
streamRates[STREAM_PARAMS].set(10);
|
||||
}
|
||||
if (stream_trigger(STREAM_PARAMS)) {
|
||||
send_message(MSG_NEXT_PARAM);
|
||||
}
|
||||
}
|
||||
|
||||
if (in_mavlink_delay) {
|
||||
// don't send any other stream types while in the delay callback
|
||||
return;
|
||||
}
|
||||
|
||||
if (stream_trigger(STREAM_RAW_SENSORS)) {
|
||||
send_message(MSG_RAW_IMU1);
|
||||
send_message(MSG_RAW_IMU2);
|
||||
send_message(MSG_RAW_IMU3);
|
||||
}
|
||||
|
||||
if (stream_trigger(STREAM_EXTENDED_STATUS)) {
|
||||
send_message(MSG_EXTENDED_STATUS1);
|
||||
send_message(MSG_EXTENDED_STATUS2);
|
||||
send_message(MSG_NAV_CONTROLLER_OUTPUT);
|
||||
send_message(MSG_GPS_RAW);
|
||||
}
|
||||
|
||||
if (stream_trigger(STREAM_POSITION)) {
|
||||
send_message(MSG_LOCATION);
|
||||
}
|
||||
|
||||
if (stream_trigger(STREAM_RAW_CONTROLLER)) {
|
||||
send_message(MSG_SERVO_OUT);
|
||||
}
|
||||
|
||||
if (stream_trigger(STREAM_RC_CHANNELS)) {
|
||||
send_message(MSG_RADIO_OUT);
|
||||
}
|
||||
|
||||
if (stream_trigger(STREAM_EXTRA1)) {
|
||||
send_message(MSG_ATTITUDE);
|
||||
}
|
||||
|
||||
if (stream_trigger(STREAM_EXTRA3)) {
|
||||
send_message(MSG_AHRS);
|
||||
send_message(MSG_HWSTATUS);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
void
|
||||
GCS_MAVLINK::send_message(enum ap_message id)
|
||||
{
|
||||
mavlink_send_message(chan,id, packet_drops);
|
||||
}
|
||||
|
||||
void
|
||||
GCS_MAVLINK::send_text_P(gcs_severity severity, const prog_char_t *str)
|
||||
{
|
||||
mavlink_statustext_t m;
|
||||
uint8_t i;
|
||||
for (i=0; i<sizeof(m.text); i++) {
|
||||
m.text[i] = pgm_read_byte((const prog_char *)(str++));
|
||||
if (m.text[i] == '\0') {
|
||||
break;
|
||||
}
|
||||
}
|
||||
if (i < sizeof(m.text)) m.text[i] = 0;
|
||||
mavlink_send_text(chan, severity, (const char *)m.text);
|
||||
}
|
||||
|
||||
void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
|
||||
{
|
||||
switch (msg->msgid) {
|
||||
|
||||
case MAVLINK_MSG_ID_REQUEST_DATA_STREAM:
|
||||
{
|
||||
// decode
|
||||
mavlink_request_data_stream_t packet;
|
||||
mavlink_msg_request_data_stream_decode(msg, &packet);
|
||||
|
||||
if (mavlink_check_target(packet.target_system, packet.target_component))
|
||||
break;
|
||||
|
||||
int16_t freq = 0; // packet frequency
|
||||
|
||||
if (packet.start_stop == 0)
|
||||
freq = 0; // stop sending
|
||||
else if (packet.start_stop == 1)
|
||||
freq = packet.req_message_rate; // start sending
|
||||
else
|
||||
break;
|
||||
|
||||
switch (packet.req_stream_id) {
|
||||
case MAV_DATA_STREAM_ALL:
|
||||
// note that we don't set STREAM_PARAMS - that is internal only
|
||||
for (uint8_t i=0; i<STREAM_PARAMS; i++) {
|
||||
streamRates[i].set_and_save_ifchanged(freq);
|
||||
}
|
||||
break;
|
||||
case MAV_DATA_STREAM_RAW_SENSORS:
|
||||
streamRates[STREAM_RAW_SENSORS].set_and_save_ifchanged(freq);
|
||||
break;
|
||||
case MAV_DATA_STREAM_EXTENDED_STATUS:
|
||||
streamRates[STREAM_EXTENDED_STATUS].set_and_save_ifchanged(freq);
|
||||
break;
|
||||
case MAV_DATA_STREAM_RC_CHANNELS:
|
||||
streamRates[STREAM_RC_CHANNELS].set_and_save_ifchanged(freq);
|
||||
break;
|
||||
case MAV_DATA_STREAM_RAW_CONTROLLER:
|
||||
streamRates[STREAM_RAW_CONTROLLER].set_and_save_ifchanged(freq);
|
||||
break;
|
||||
case MAV_DATA_STREAM_POSITION:
|
||||
streamRates[STREAM_POSITION].set_and_save_ifchanged(freq);
|
||||
break;
|
||||
case MAV_DATA_STREAM_EXTRA1:
|
||||
streamRates[STREAM_EXTRA1].set_and_save_ifchanged(freq);
|
||||
break;
|
||||
case MAV_DATA_STREAM_EXTRA2:
|
||||
streamRates[STREAM_EXTRA2].set_and_save_ifchanged(freq);
|
||||
break;
|
||||
case MAV_DATA_STREAM_EXTRA3:
|
||||
streamRates[STREAM_EXTRA3].set_and_save_ifchanged(freq);
|
||||
break;
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
||||
|
||||
case MAVLINK_MSG_ID_PARAM_REQUEST_LIST:
|
||||
{
|
||||
// decode
|
||||
mavlink_param_request_list_t packet;
|
||||
mavlink_msg_param_request_list_decode(msg, &packet);
|
||||
if (mavlink_check_target(packet.target_system,packet.target_component)) break;
|
||||
|
||||
// Start sending parameters - next call to ::update will kick the first one out
|
||||
|
||||
_queued_parameter = AP_Param::first(&_queued_parameter_token, &_queued_parameter_type);
|
||||
_queued_parameter_index = 0;
|
||||
_queued_parameter_count = _count_parameters();
|
||||
break;
|
||||
}
|
||||
|
||||
case MAVLINK_MSG_ID_PARAM_REQUEST_READ:
|
||||
{
|
||||
// decode
|
||||
mavlink_param_request_read_t packet;
|
||||
mavlink_msg_param_request_read_decode(msg, &packet);
|
||||
if (mavlink_check_target(packet.target_system,packet.target_component)) break;
|
||||
enum ap_var_type p_type;
|
||||
AP_Param *vp;
|
||||
char param_name[AP_MAX_NAME_SIZE+1];
|
||||
if (packet.param_index != -1) {
|
||||
AP_Param::ParamToken token;
|
||||
vp = AP_Param::find_by_index(packet.param_index, &p_type, &token);
|
||||
if (vp == NULL) {
|
||||
gcs_send_text_fmt(PSTR("Unknown parameter index %d"), packet.param_index);
|
||||
break;
|
||||
}
|
||||
vp->copy_name_token(token, param_name, AP_MAX_NAME_SIZE, true);
|
||||
param_name[AP_MAX_NAME_SIZE] = 0;
|
||||
} else {
|
||||
strncpy(param_name, packet.param_id, AP_MAX_NAME_SIZE);
|
||||
param_name[AP_MAX_NAME_SIZE] = 0;
|
||||
vp = AP_Param::find(param_name, &p_type);
|
||||
if (vp == NULL) {
|
||||
gcs_send_text_fmt(PSTR("Unknown parameter %.16s"), packet.param_id);
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
float value = vp->cast_to_float(p_type);
|
||||
mavlink_msg_param_value_send(
|
||||
chan,
|
||||
param_name,
|
||||
value,
|
||||
mav_var_type(p_type),
|
||||
_count_parameters(),
|
||||
packet.param_index);
|
||||
break;
|
||||
}
|
||||
|
||||
case MAVLINK_MSG_ID_PARAM_SET:
|
||||
{
|
||||
AP_Param *vp;
|
||||
enum ap_var_type var_type;
|
||||
|
||||
// decode
|
||||
mavlink_param_set_t packet;
|
||||
mavlink_msg_param_set_decode(msg, &packet);
|
||||
|
||||
if (mavlink_check_target(packet.target_system, packet.target_component))
|
||||
break;
|
||||
|
||||
// set parameter
|
||||
|
||||
char key[AP_MAX_NAME_SIZE+1];
|
||||
strncpy(key, (char *)packet.param_id, AP_MAX_NAME_SIZE);
|
||||
key[AP_MAX_NAME_SIZE] = 0;
|
||||
|
||||
// find the requested parameter
|
||||
vp = AP_Param::find(key, &var_type);
|
||||
if ((NULL != vp) && // exists
|
||||
!isnan(packet.param_value) && // not nan
|
||||
!isinf(packet.param_value)) { // not inf
|
||||
|
||||
// add a small amount before casting parameter values
|
||||
// from float to integer to avoid truncating to the
|
||||
// next lower integer value.
|
||||
float rounding_addition = 0.01;
|
||||
|
||||
// handle variables with standard type IDs
|
||||
if (var_type == AP_PARAM_FLOAT) {
|
||||
((AP_Float *)vp)->set_and_save(packet.param_value);
|
||||
} else if (var_type == AP_PARAM_INT32) {
|
||||
if (packet.param_value < 0) rounding_addition = -rounding_addition;
|
||||
float v = packet.param_value+rounding_addition;
|
||||
v = constrain_float(v, -2147483648.0, 2147483647.0);
|
||||
((AP_Int32 *)vp)->set_and_save(v);
|
||||
} else if (var_type == AP_PARAM_INT16) {
|
||||
if (packet.param_value < 0) rounding_addition = -rounding_addition;
|
||||
float v = packet.param_value+rounding_addition;
|
||||
v = constrain_float(v, -32768, 32767);
|
||||
((AP_Int16 *)vp)->set_and_save(v);
|
||||
} else if (var_type == AP_PARAM_INT8) {
|
||||
if (packet.param_value < 0) rounding_addition = -rounding_addition;
|
||||
float v = packet.param_value+rounding_addition;
|
||||
v = constrain_float(v, -128, 127);
|
||||
((AP_Int8 *)vp)->set_and_save(v);
|
||||
} else {
|
||||
// we don't support mavlink set on this parameter
|
||||
break;
|
||||
}
|
||||
|
||||
// Report back the new value if we accepted the change
|
||||
// we send the value we actually set, which could be
|
||||
// different from the value sent, in case someone sent
|
||||
// a fractional value to an integer type
|
||||
mavlink_msg_param_value_send(
|
||||
chan,
|
||||
key,
|
||||
vp->cast_to_float(var_type),
|
||||
mav_var_type(var_type),
|
||||
_count_parameters(),
|
||||
-1); // XXX we don't actually know what its index is...
|
||||
#if LOGGING_ENABLED == ENABLED
|
||||
DataFlash.Log_Write_Parameter(key, vp->cast_to_float(var_type));
|
||||
#endif
|
||||
}
|
||||
|
||||
break;
|
||||
} // end case
|
||||
|
||||
case MAVLINK_MSG_ID_HEARTBEAT:
|
||||
{
|
||||
if (msg->sysid != g.sysid_my_gcs) break;
|
||||
break;
|
||||
}
|
||||
|
||||
case MAVLINK_MSG_ID_GLOBAL_POSITION_INT: {
|
||||
// decode
|
||||
mavlink_global_position_int_t packet;
|
||||
mavlink_msg_global_position_int_decode(msg, &packet);
|
||||
tracking_update_position(packet);
|
||||
break;
|
||||
}
|
||||
|
||||
default:
|
||||
break;
|
||||
|
||||
} // end switch
|
||||
} // end handle mavlink
|
||||
|
||||
uint16_t
|
||||
GCS_MAVLINK::_count_parameters()
|
||||
{
|
||||
// if we haven't cached the parameter count yet...
|
||||
if (0 == _parameter_count) {
|
||||
AP_Param *vp;
|
||||
AP_Param::ParamToken token;
|
||||
|
||||
vp = AP_Param::first(&token, NULL);
|
||||
do {
|
||||
_parameter_count++;
|
||||
} while (NULL != (vp = AP_Param::next_scalar(&token, NULL)));
|
||||
}
|
||||
return _parameter_count;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Send the next pending parameter, called from deferred message
|
||||
* handling code
|
||||
*/
|
||||
void
|
||||
GCS_MAVLINK::queued_param_send()
|
||||
{
|
||||
if (_queued_parameter == NULL) {
|
||||
return;
|
||||
}
|
||||
|
||||
uint16_t bytes_allowed;
|
||||
uint8_t count;
|
||||
uint32_t tnow = hal.scheduler->millis();
|
||||
|
||||
// use at most 30% of bandwidth on parameters. The constant 26 is
|
||||
// 1/(1000 * 1/8 * 0.001 * 0.3)
|
||||
bytes_allowed = g.serial3_baud * (tnow - _queued_parameter_send_time_ms) * 26;
|
||||
if (bytes_allowed > comm_get_txspace(chan)) {
|
||||
bytes_allowed = comm_get_txspace(chan);
|
||||
}
|
||||
count = bytes_allowed / (MAVLINK_MSG_ID_PARAM_VALUE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES);
|
||||
|
||||
while (_queued_parameter != NULL && count--) {
|
||||
AP_Param *vp;
|
||||
float value;
|
||||
|
||||
// copy the current parameter and prepare to move to the next
|
||||
vp = _queued_parameter;
|
||||
|
||||
// if the parameter can be cast to float, report it here and break out of the loop
|
||||
value = vp->cast_to_float(_queued_parameter_type);
|
||||
|
||||
char param_name[AP_MAX_NAME_SIZE];
|
||||
vp->copy_name_token(_queued_parameter_token, param_name, sizeof(param_name), true);
|
||||
|
||||
mavlink_msg_param_value_send(
|
||||
chan,
|
||||
param_name,
|
||||
value,
|
||||
mav_var_type(_queued_parameter_type),
|
||||
_queued_parameter_count,
|
||||
_queued_parameter_index);
|
||||
|
||||
_queued_parameter = AP_Param::next_scalar(&_queued_parameter_token, &_queued_parameter_type);
|
||||
_queued_parameter_index++;
|
||||
}
|
||||
_queued_parameter_send_time_ms = tnow;
|
||||
}
|
||||
|
||||
/*
|
||||
* a delay() callback that processes MAVLink packets. We set this as the
|
||||
* callback in long running library initialisation routines to allow
|
||||
* MAVLink to process packets while waiting for the initialisation to
|
||||
* complete
|
||||
*/
|
||||
static void mavlink_delay_cb()
|
||||
{
|
||||
static uint32_t last_1hz, last_50hz, last_5s;
|
||||
if (!gcs0.initialised) return;
|
||||
|
||||
in_mavlink_delay = true;
|
||||
|
||||
uint32_t tnow = hal.scheduler->millis();
|
||||
if (tnow - last_1hz > 1000) {
|
||||
last_1hz = tnow;
|
||||
gcs_send_message(MSG_HEARTBEAT);
|
||||
gcs_send_message(MSG_EXTENDED_STATUS1);
|
||||
}
|
||||
if (tnow - last_50hz > 20) {
|
||||
last_50hz = tnow;
|
||||
gcs_update();
|
||||
gcs_data_stream_send();
|
||||
notify.update();
|
||||
}
|
||||
if (tnow - last_5s > 5000) {
|
||||
last_5s = tnow;
|
||||
gcs_send_text_P(SEVERITY_LOW, PSTR("Initialising APM..."));
|
||||
}
|
||||
in_mavlink_delay = false;
|
||||
}
|
||||
|
||||
/*
|
||||
* send a message on both GCS links
|
||||
*/
|
||||
static void gcs_send_message(enum ap_message id)
|
||||
{
|
||||
gcs0.send_message(id);
|
||||
if (gcs3.initialised) {
|
||||
gcs3.send_message(id);
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
* send data streams in the given rate range on both links
|
||||
*/
|
||||
static void gcs_data_stream_send(void)
|
||||
{
|
||||
gcs0.data_stream_send();
|
||||
if (gcs3.initialised) {
|
||||
gcs3.data_stream_send();
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
* look for incoming commands on the GCS links
|
||||
*/
|
||||
static void gcs_update(void)
|
||||
{
|
||||
gcs0.update();
|
||||
if (gcs3.initialised) {
|
||||
gcs3.update();
|
||||
}
|
||||
}
|
||||
|
||||
static void gcs_send_text_P(gcs_severity severity, const prog_char_t *str)
|
||||
{
|
||||
gcs0.send_text_P(severity, str);
|
||||
if (gcs3.initialised) {
|
||||
gcs3.send_text_P(severity, str);
|
||||
}
|
||||
#if LOGGING_ENABLED == ENABLED
|
||||
DataFlash.Log_Write_Message_P(str);
|
||||
#endif
|
||||
}
|
||||
|
||||
/*
|
||||
* send a low priority formatted message to the GCS
|
||||
* 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, ...)
|
||||
{
|
||||
va_list arg_list;
|
||||
gcs0.pending_status.severity = (uint8_t)SEVERITY_LOW;
|
||||
va_start(arg_list, fmt);
|
||||
hal.util->vsnprintf_P((char *)gcs0.pending_status.text,
|
||||
sizeof(gcs0.pending_status.text), fmt, arg_list);
|
||||
va_end(arg_list);
|
||||
#if LOGGING_ENABLED == ENABLED
|
||||
DataFlash.Log_Write_Message(gcs0.pending_status.text);
|
||||
#endif
|
||||
gcs3.pending_status = gcs0.pending_status;
|
||||
mavlink_send_message(MAVLINK_COMM_0, MSG_STATUSTEXT, 0);
|
||||
if (gcs3.initialised) {
|
||||
mavlink_send_message(MAVLINK_COMM_1, MSG_STATUSTEXT, 0);
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -0,0 +1 @@
|
|||
include ../../mk/apm.mk
|
||||
|
|
@ -0,0 +1,101 @@
|
|||
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
||||
|
||||
#ifndef PARAMETERS_H
|
||||
#define PARAMETERS_H
|
||||
|
||||
#include <AP_Common.h>
|
||||
|
||||
// Global parameter class.
|
||||
//
|
||||
class Parameters {
|
||||
public:
|
||||
|
||||
/*
|
||||
* The value of k_format_version determines whether the existing
|
||||
* eeprom data is considered valid. You should only change this
|
||||
* value under the following circumstances:
|
||||
*
|
||||
* 1) the meaning of an existing eeprom parameter changes
|
||||
*
|
||||
* 2) the value of an existing k_param_* enum value changes
|
||||
*
|
||||
* Adding a new parameter should _not_ require a change to
|
||||
* k_format_version except under special circumstances. If you
|
||||
* change it anyway then all ArduPlane users will need to reload all
|
||||
* their parameters. We want that to be an extremely rare
|
||||
* thing. Please do not just change it "just in case".
|
||||
*
|
||||
* To determine if a k_param_* value has changed, use the rules of
|
||||
* C++ enums to work out the value of the neighboring enum
|
||||
* values. If you don't know the C++ enum rules then please ask for
|
||||
* help.
|
||||
*/
|
||||
|
||||
//////////////////////////////////////////////////////////////////
|
||||
// STOP!!! DO NOT CHANGE THIS VALUE UNTIL YOU FULLY UNDERSTAND THE
|
||||
// COMMENTS ABOVE. IF UNSURE, ASK ANOTHER DEVELOPER!!!
|
||||
static const uint16_t k_format_version = 1;
|
||||
//////////////////////////////////////////////////////////////////
|
||||
|
||||
|
||||
// The parameter software_type is set up solely for ground station use
|
||||
// and identifies the software type (eg ArduPilotMega versus ArduCopterMega)
|
||||
// GCS will interpret values 0-9 as ArduPilotMega. Developers may use
|
||||
// values within that range to identify different branches.
|
||||
//
|
||||
static const uint16_t k_software_type = 4;
|
||||
|
||||
enum {
|
||||
// Layout version number, always key zero.
|
||||
//
|
||||
k_param_format_version = 0,
|
||||
k_param_software_type,
|
||||
|
||||
k_param_gcs0 = 100, // stream rates for port0
|
||||
k_param_gcs3, // stream rates for port3
|
||||
k_param_sysid_this_mav,
|
||||
k_param_sysid_my_gcs,
|
||||
k_param_serial0_baud,
|
||||
k_param_serial3_baud,
|
||||
k_param_imu,
|
||||
k_param_compass_enabled,
|
||||
k_param_compass,
|
||||
k_param_ahrs, // AHRS group
|
||||
k_param_barometer,
|
||||
k_param_scheduler,
|
||||
k_param_ins,
|
||||
k_param_sitl,
|
||||
k_param_pidPitch2Srv,
|
||||
k_param_pidYaw2Srv,
|
||||
|
||||
k_param_channel_yaw = 200,
|
||||
k_param_channel_pitch
|
||||
|
||||
// 254,255: reserved
|
||||
};
|
||||
|
||||
AP_Int16 format_version;
|
||||
AP_Int8 software_type;
|
||||
|
||||
// Telemetry control
|
||||
//
|
||||
AP_Int16 sysid_this_mav;
|
||||
AP_Int16 sysid_my_gcs;
|
||||
AP_Int8 serial0_baud;
|
||||
AP_Int8 serial3_baud;
|
||||
|
||||
AP_Int8 compass_enabled;
|
||||
|
||||
// PID controllers
|
||||
PID pidPitch2Srv;
|
||||
PID pidYaw2Srv;
|
||||
|
||||
Parameters() :
|
||||
pidPitch2Srv(1.0f, 0.2f, 0.05f, 4000.0f),
|
||||
pidYaw2Srv(1.0f, 0.2f, 0.05f, 4000.0f)
|
||||
{}
|
||||
};
|
||||
|
||||
extern const AP_Param::Info var_info[];
|
||||
|
||||
#endif // PARAMETERS_H
|
||||
|
|
@ -0,0 +1,125 @@
|
|||
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
||||
|
||||
/*
|
||||
* 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} }
|
||||
|
||||
const AP_Param::Info var_info[] PROGMEM = {
|
||||
GSCALAR(format_version, "FORMAT_VERSION", 0),
|
||||
GSCALAR(software_type, "SYSID_SW_TYPE", Parameters::k_software_type),
|
||||
|
||||
// @Param: SYSID_THISMAV
|
||||
// @DisplayName: MAVLink system ID
|
||||
// @Description: The identifier of this device in the MAVLink protocol
|
||||
// @Range: 1 255
|
||||
// @User: Advanced
|
||||
GSCALAR(sysid_this_mav, "SYSID_THISMAV", MAV_SYSTEM_ID),
|
||||
|
||||
// @Param: SYSID_MYGCS
|
||||
// @DisplayName: Ground station MAVLink system ID
|
||||
// @Description: The identifier of the ground station in the MAVLink protocol. Don't change this unless you also modify the ground station to match.
|
||||
// @Range: 1 255
|
||||
// @User: Advanced
|
||||
GSCALAR(sysid_my_gcs, "SYSID_MYGCS", 255),
|
||||
|
||||
// @Param: SERIAL0_BAUD
|
||||
// @DisplayName: USB Console Baud Rate
|
||||
// @Description: The baud rate used on the main uart
|
||||
// @Values: 1:1200,2:2400,4:4800,9:9600,19:19200,38:38400,57:57600,111:111100,115:115200
|
||||
// @User: Standard
|
||||
GSCALAR(serial0_baud, "SERIAL0_BAUD", SERIAL0_BAUD/1000),
|
||||
|
||||
// @Param: SERIAL3_BAUD
|
||||
// @DisplayName: Telemetry Baud Rate
|
||||
// @Description: The baud rate used on the telemetry port
|
||||
// @Values: 1:1200,2:2400,4:4800,9:9600,19:19200,38:38400,57:57600,111:111100,115:115200
|
||||
// @User: Standard
|
||||
GSCALAR(serial3_baud, "SERIAL3_BAUD", SERIAL3_BAUD/1000),
|
||||
|
||||
|
||||
|
||||
// @Param: MAG_ENABLE
|
||||
// @DisplayName: Enable Compass
|
||||
// @Description: Setting this to Enabled(1) will enable the compass. Setting this to Disabled(0) will disable the compass. Note that this is separate from COMPASS_USE. This will enable the low level senor, and will enable logging of magnetometer data. To use the compass for navigation you must also set COMPASS_USE to 1.
|
||||
// @Values: 0:Disabled,1:Enabled
|
||||
// @User: Standard
|
||||
GSCALAR(compass_enabled, "MAG_ENABLE", 1),
|
||||
|
||||
// barometer ground calibration. The GND_ prefix is chosen for
|
||||
// compatibility with previous releases of ArduPlane
|
||||
// @Group: GND_
|
||||
// @Path: ../libraries/AP_Baro/AP_Baro.cpp
|
||||
GOBJECT(barometer, "GND_", AP_Baro),
|
||||
|
||||
// @Group: COMPASS_
|
||||
// @Path: ../libraries/AP_Compass/Compass.cpp
|
||||
GOBJECT(compass, "COMPASS_", Compass),
|
||||
|
||||
// @Group: SCHED_
|
||||
// @Path: ../libraries/AP_Scheduler/AP_Scheduler.cpp
|
||||
GOBJECT(scheduler, "SCHED_", AP_Scheduler),
|
||||
|
||||
// @Group: SR0_
|
||||
// @Path: GCS_Mavlink.pde
|
||||
GOBJECT(gcs0, "SR0_", GCS_MAVLINK),
|
||||
|
||||
// @Group: SR3_
|
||||
// @Path: GCS_Mavlink.pde
|
||||
GOBJECT(gcs3, "SR3_", GCS_MAVLINK),
|
||||
|
||||
// @Group: INS_
|
||||
// @Path: ../libraries/AP_InertialSensor/AP_InertialSensor.cpp
|
||||
GOBJECT(ins, "INS_", AP_InertialSensor),
|
||||
|
||||
// @Group: AHRS_
|
||||
// @Path: ../libraries/AP_AHRS/AP_AHRS.cpp
|
||||
GOBJECT(ahrs, "AHRS_", AP_AHRS),
|
||||
|
||||
#if CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL
|
||||
// @Group: SIM_
|
||||
// @Path: ../libraries/SITL/SITL.cpp
|
||||
GOBJECT(sitl, "SIM_", SITL),
|
||||
#endif
|
||||
|
||||
// RC channel
|
||||
//-----------
|
||||
// @Group: RC1_
|
||||
// @Path: ../libraries/RC_Channel/RC_Channel.cpp
|
||||
GOBJECT(channel_yaw, "RC1_", RC_Channel),
|
||||
|
||||
// @Group: RC2_
|
||||
// @Path: ../libraries/RC_Channel/RC_Channel.cpp
|
||||
GOBJECT(channel_pitch, "RC2_", RC_Channel),
|
||||
|
||||
GGROUP(pidPitch2Srv, "PITCH2SRV_", PID),
|
||||
GGROUP(pidYaw2Srv, "YAW2SRV_", PID),
|
||||
|
||||
AP_VAREND
|
||||
};
|
||||
|
||||
|
||||
static void 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"));
|
||||
AP_Param::erase_all();
|
||||
|
||||
// save the current format version
|
||||
g.format_version.set_and_save(Parameters::k_format_version);
|
||||
cliSerial->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);
|
||||
}
|
||||
}
|
||||
|
|
@ -0,0 +1,83 @@
|
|||
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
||||
//
|
||||
#include "defines.h"
|
||||
|
||||
#include "APM_Config.h" // <== THIS INCLUDE, DO NOT EDIT IT. EVER.
|
||||
|
||||
///
|
||||
/// DO NOT EDIT THIS INCLUDE - if you want to make a local change, make that
|
||||
/// change in your local copy of APM_Config.h.
|
||||
///
|
||||
|
||||
// Just so that it's completely clear...
|
||||
#define ENABLED 1
|
||||
#define DISABLED 0
|
||||
|
||||
// this avoids a very common config error
|
||||
#define ENABLE ENABLED
|
||||
#define DISABLE DISABLED
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////
|
||||
// main board differences
|
||||
//
|
||||
#if CONFIG_HAL_BOARD == HAL_BOARD_APM1
|
||||
# define CONFIG_INS_TYPE CONFIG_INS_OILPAN
|
||||
# define CONFIG_BARO AP_BARO_BMP085
|
||||
# define CONFIG_COMPASS AP_COMPASS_HMC5843
|
||||
#elif CONFIG_HAL_BOARD == HAL_BOARD_APM2
|
||||
# define CONFIG_INS_TYPE CONFIG_INS_MPU6000
|
||||
# define CONFIG_BARO AP_BARO_MS5611
|
||||
# define CONFIG_MS5611_SERIAL AP_BARO_MS5611_SPI
|
||||
# define CONFIG_COMPASS AP_COMPASS_HMC5843
|
||||
#elif CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL
|
||||
# define CONFIG_INS_TYPE CONFIG_INS_HIL
|
||||
# define CONFIG_BARO AP_BARO_HIL
|
||||
# define CONFIG_COMPASS AP_COMPASS_HIL
|
||||
#elif CONFIG_HAL_BOARD == HAL_BOARD_PX4
|
||||
# define CONFIG_INS_TYPE CONFIG_INS_PX4
|
||||
# define CONFIG_BARO AP_BARO_PX4
|
||||
# define CONFIG_COMPASS AP_COMPASS_PX4
|
||||
# define SERIAL0_BAUD 115200
|
||||
#elif CONFIG_HAL_BOARD == HAL_BOARD_FLYMAPLE
|
||||
# define CONFIG_INS_TYPE CONFIG_INS_FLYMAPLE
|
||||
# define CONFIG_BARO AP_BARO_BMP085
|
||||
# define CONFIG_COMPASS AP_COMPASS_HMC5843
|
||||
# define SERIAL0_BAUD 115200
|
||||
#elif CONFIG_HAL_BOARD == HAL_BOARD_LINUX
|
||||
# define CONFIG_INS_TYPE CONFIG_INS_L3G4200D
|
||||
# define CONFIG_BARO AP_BARO_BMP085
|
||||
# define CONFIG_COMPASS AP_COMPASS_HMC5843
|
||||
#endif
|
||||
|
||||
|
||||
#ifndef CONFIG_BARO
|
||||
# error "CONFIG_BARO not set"
|
||||
#endif
|
||||
|
||||
#ifndef CONFIG_COMPASS
|
||||
# error "CONFIG_COMPASS not set"
|
||||
#endif
|
||||
|
||||
#ifndef MAV_SYSTEM_ID
|
||||
// use 2 for antenna tracker by default
|
||||
# define MAV_SYSTEM_ID 2
|
||||
#endif
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////
|
||||
// Serial port speeds.
|
||||
//
|
||||
#ifndef SERIAL0_BAUD
|
||||
# define SERIAL0_BAUD 115200
|
||||
#endif
|
||||
#ifndef SERIAL3_BAUD
|
||||
# define SERIAL3_BAUD 57600
|
||||
#endif
|
||||
|
||||
|
||||
#ifndef SERIAL_BUFSIZE
|
||||
# define SERIAL_BUFSIZE 512
|
||||
#endif
|
||||
|
||||
#ifndef SERIAL2_BUFSIZE
|
||||
# define SERIAL2_BUFSIZE 256
|
||||
#endif
|
||||
|
|
@ -0,0 +1,56 @@
|
|||
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
||||
|
||||
#ifndef _DEFINES_H
|
||||
#define _DEFINES_H
|
||||
|
||||
#define ToRad(x) radians(x) // *pi/180
|
||||
#define ToDeg(x) degrees(x) // *180/pi
|
||||
|
||||
/// please keep each MSG_ to a single MAVLink message. If need be
|
||||
/// create new MSG_ IDs for additional messages on the same
|
||||
/// stream
|
||||
enum ap_message {
|
||||
MSG_HEARTBEAT,
|
||||
MSG_ATTITUDE,
|
||||
MSG_LOCATION,
|
||||
MSG_AHRS,
|
||||
MSG_HWSTATUS,
|
||||
MSG_GPS_RAW,
|
||||
MSG_SERVO_OUT,
|
||||
MSG_RADIO_OUT,
|
||||
MSG_RAW_IMU1,
|
||||
MSG_RAW_IMU2,
|
||||
MSG_RAW_IMU3,
|
||||
MSG_NEXT_PARAM,
|
||||
MSG_STATUSTEXT,
|
||||
MSG_EXTENDED_STATUS1,
|
||||
MSG_EXTENDED_STATUS2,
|
||||
MSG_NAV_CONTROLLER_OUTPUT,
|
||||
MSG_RETRY_DEFERRED // this must be last
|
||||
};
|
||||
|
||||
#define EEPROM_MAX_ADDR 4096
|
||||
|
||||
// mark a function as not to be inlined
|
||||
#define NOINLINE __attribute__((noinline))
|
||||
|
||||
// InertialSensor driver types
|
||||
#define CONFIG_INS_OILPAN 1
|
||||
#define CONFIG_INS_MPU6000 2
|
||||
#define CONFIG_INS_HIL 3
|
||||
#define CONFIG_INS_PX4 4
|
||||
#define CONFIG_INS_FLYMAPLE 5
|
||||
#define CONFIG_INS_L3G4200D 6
|
||||
|
||||
// barometer driver types
|
||||
#define AP_BARO_BMP085 1
|
||||
#define AP_BARO_MS5611 2
|
||||
#define AP_BARO_PX4 3
|
||||
#define AP_BARO_HIL 4
|
||||
|
||||
// compass driver types
|
||||
#define AP_COMPASS_HMC5843 1
|
||||
#define AP_COMPASS_PX4 2
|
||||
#define AP_COMPASS_HIL 3
|
||||
|
||||
#endif // _DEFINES_H
|
||||
|
|
@ -0,0 +1,65 @@
|
|||
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
||||
|
||||
static void init_barometer(void)
|
||||
{
|
||||
gcs_send_text_P(SEVERITY_LOW, PSTR("Calibrating barometer"));
|
||||
barometer.calibrate();
|
||||
gcs_send_text_P(SEVERITY_LOW, PSTR("barometer calibration complete"));
|
||||
}
|
||||
|
||||
// read the barometer and return the updated altitude in meters
|
||||
static void update_barometer(void)
|
||||
{
|
||||
barometer.read();
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
update INS and attitude
|
||||
*/
|
||||
static void update_ahrs()
|
||||
{
|
||||
ahrs.update();
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
read and update compass
|
||||
*/
|
||||
static void update_compass(void)
|
||||
{
|
||||
if (g.compass_enabled && compass.read()) {
|
||||
ahrs.set_compass(&compass);
|
||||
compass.null_offsets();
|
||||
} else {
|
||||
ahrs.set_compass(NULL);
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
if the compass is enabled then try to accumulate a reading
|
||||
*/
|
||||
static void compass_accumulate(void)
|
||||
{
|
||||
if (g.compass_enabled) {
|
||||
compass.accumulate();
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
try to accumulate a baro reading
|
||||
*/
|
||||
static void barometer_accumulate(void)
|
||||
{
|
||||
barometer.accumulate();
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
read the GPS
|
||||
*/
|
||||
static void update_GPS(void)
|
||||
{
|
||||
g_gps->update();
|
||||
}
|
||||
|
||||
|
|
@ -0,0 +1,109 @@
|
|||
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
||||
|
||||
static void init_tracker()
|
||||
{
|
||||
hal.uartA->begin(SERIAL0_BAUD, 128, SERIAL_BUFSIZE);
|
||||
|
||||
// gps port
|
||||
hal.uartB->begin(38400, 256, 16);
|
||||
|
||||
cliSerial->printf_P(PSTR("\n\nInit " THISFIRMWARE
|
||||
"\n\nFree RAM: %u\n"),
|
||||
memcheck_available_memory());
|
||||
|
||||
// Check the EEPROM format version before loading any parameters from EEPROM
|
||||
load_parameters();
|
||||
|
||||
// reset the uartA baud rate after parameter load
|
||||
hal.uartA->begin(map_baudrate(g.serial0_baud, SERIAL0_BAUD));
|
||||
|
||||
// init baro before we start the GCS, so that the CLI baro test works
|
||||
barometer.init();
|
||||
|
||||
// init the GCS
|
||||
gcs0.init(hal.uartA);
|
||||
// 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);
|
||||
|
||||
// we have a 2nd serial port for telemetry
|
||||
hal.uartC->begin(map_baudrate(g.serial3_baud, SERIAL3_BAUD),
|
||||
128, SERIAL2_BUFSIZE);
|
||||
gcs3.init(hal.uartC);
|
||||
|
||||
mavlink_system.sysid = g.sysid_this_mav;
|
||||
|
||||
if (g.compass_enabled==true) {
|
||||
if (!compass.init() || !compass.read()) {
|
||||
cliSerial->println_P(PSTR("Compass initialisation failed!"));
|
||||
g.compass_enabled = false;
|
||||
} else {
|
||||
ahrs.set_compass(&compass);
|
||||
}
|
||||
}
|
||||
|
||||
// Do GPS init
|
||||
g_gps = &g_gps_driver;
|
||||
|
||||
// GPS Initialization
|
||||
g_gps->init(hal.uartB, GPS::GPS_ENGINE_STATIONARY);
|
||||
|
||||
mavlink_system.compid = 4;
|
||||
mavlink_system.type = MAV_TYPE_ANTENNA_TRACKER;
|
||||
|
||||
ahrs.init();
|
||||
ahrs.set_fly_forward(false);
|
||||
|
||||
ins.init(AP_InertialSensor::WARM_START, ins_sample_rate);
|
||||
ahrs.reset();
|
||||
|
||||
init_barometer();
|
||||
|
||||
hal.uartA->set_blocking_writes(false);
|
||||
hal.uartC->set_blocking_writes(false);
|
||||
|
||||
// setup antenna control PWM channels
|
||||
channel_yaw.set_angle(4500);
|
||||
channel_pitch.set_angle(4500);
|
||||
|
||||
channel_yaw.output_trim();
|
||||
channel_pitch.output_trim();
|
||||
|
||||
channel_yaw.calc_pwm();
|
||||
channel_pitch.calc_pwm();
|
||||
|
||||
channel_yaw.enable_out();
|
||||
channel_pitch.enable_out();
|
||||
|
||||
gcs_send_text_P(SEVERITY_LOW,PSTR("\nReady to track."));
|
||||
hal.scheduler->delay(1000);
|
||||
}
|
||||
|
||||
// updates the status of the notify objects
|
||||
// should be called at 50hz
|
||||
static void update_notify()
|
||||
{
|
||||
notify.update();
|
||||
}
|
||||
|
||||
/*
|
||||
* map from a 8 bit EEPROM baud rate to a real baud rate
|
||||
*/
|
||||
static uint32_t map_baudrate(int8_t rate, uint32_t default_baud)
|
||||
{
|
||||
switch (rate) {
|
||||
case 1: return 1200;
|
||||
case 2: return 2400;
|
||||
case 4: return 4800;
|
||||
case 9: return 9600;
|
||||
case 19: return 19200;
|
||||
case 38: return 38400;
|
||||
case 57: return 57600;
|
||||
case 111: return 111100;
|
||||
case 115: return 115200;
|
||||
}
|
||||
cliSerial->println_P(PSTR("Invalid SERIAL3_BAUD"));
|
||||
return default_baud;
|
||||
}
|
||||
|
||||
|
||||
|
|
@ -0,0 +1,81 @@
|
|||
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
||||
|
||||
/**
|
||||
state of the vehicle we are tracking
|
||||
*/
|
||||
static struct {
|
||||
Location location;
|
||||
uint32_t last_update_us;
|
||||
float heading; // degrees
|
||||
float ground_speed; // m/s
|
||||
} vehicle;
|
||||
|
||||
static Location our_location;
|
||||
|
||||
/**
|
||||
update the pitch servo. The aim is to drive the boards pitch to the
|
||||
requested pitch
|
||||
*/
|
||||
static void update_pitch_servo(float pitch)
|
||||
{
|
||||
channel_pitch.servo_out = g.pidPitch2Srv.get_pid_4500(degrees(ahrs.pitch) - pitch);
|
||||
channel_pitch.calc_pwm();
|
||||
channel_pitch.output();
|
||||
}
|
||||
|
||||
/**
|
||||
update the yaw servo
|
||||
*/
|
||||
static void update_yaw_servo(float yaw)
|
||||
{
|
||||
channel_yaw.servo_out = g.pidYaw2Srv.get_pid_4500(degrees(ahrs.yaw) - yaw);
|
||||
channel_yaw.calc_pwm();
|
||||
channel_yaw.output();
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
main antenna tracking code, called at 50Hz
|
||||
*/
|
||||
static void update_tracking(void)
|
||||
{
|
||||
// project the vehicle position to take account of lost radio packets
|
||||
Location vpos = vehicle.location;
|
||||
float dt = (hal.scheduler->micros() - vehicle.last_update_us) * 1.0e-6f;
|
||||
location_update(vpos, vehicle.heading, vehicle.ground_speed * dt);
|
||||
|
||||
// update our position if we have at least a 2D fix
|
||||
if (g_gps->status() >= GPS::GPS_OK_FIX_2D) {
|
||||
our_location.lat = g_gps->latitude;
|
||||
our_location.lng = g_gps->longitude;
|
||||
our_location.alt = 0; // assume ground level for now
|
||||
}
|
||||
|
||||
// calculate the bearing to the vehicle
|
||||
float bearing = get_bearing_cd(our_location, vehicle.location) * 0.01f;
|
||||
float distance = get_distance(our_location, vehicle.location);
|
||||
float pitch = degrees(atan2(vehicle.location.alt - our_location.alt, distance));
|
||||
|
||||
// update the servos
|
||||
update_pitch_servo(pitch);
|
||||
update_yaw_servo(bearing);
|
||||
|
||||
// update nav_status for NAV_CONTROLLER_OUTPUT
|
||||
nav_status.bearing = bearing;
|
||||
nav_status.pitch = pitch;
|
||||
nav_status.distance = distance;
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
handle an updated position from the aircraft
|
||||
*/
|
||||
static void tracking_update_position(const mavlink_global_position_int_t &msg)
|
||||
{
|
||||
vehicle.location.lat = msg.lat;
|
||||
vehicle.location.lng = msg.lon;
|
||||
vehicle.location.alt = msg.relative_alt/10;
|
||||
vehicle.heading = msg.hdg * 0.01f;
|
||||
vehicle.ground_speed = pythagorous2(msg.vx, msg.vy) * 0.01f;
|
||||
vehicle.last_update_us = hal.scheduler->micros();
|
||||
}
|
||||
Loading…
Reference in New Issue