Skip to content
Snippets Groups Projects

Compare revisions

Changes are shown as if the source revision was being merged into the target revision. Learn more about comparing revisions.

Source

Select target project
No results found

Target

Select target project
  • opensource/ardupilot
  • opensource/baitboat
2 results
Show changes
Commits on Source (552)
452 additional commits have been omitted to prevent performance issues.
Hide whitespace changes
Inline Side-by-side
Showing
with 156 additions and 554 deletions
.gitignore
View file @ 62e7778b
......@@ -57,6 +57,7 @@ ArduCopter/test/*
ArduPlane/terrain/*.DAT
ArduPlane/test.ArduPlane/
APMrover2/test.APMrover2/
autotest.lck
build
Build.ArduCopter/*
Build.ArduPlane/*
......@@ -68,6 +69,7 @@ CMakeFiles
config.mk
dataflash.bin
eeprom.bin
index.html
LASTLOG.TXT
Make.dep
mav.log
......
.travis.yml
View file @ 62e7778b
language: cpp
before_install:
- cd .. && ./ardupilot/Tools/scripts/install-prereqs-ubuntu.sh -y && . ~/.profile
- APMDIR=$(pwd) && pushd .. && $APMDIR/Tools/scripts/install-prereqs-ubuntu.sh -y && . ~/.profile && popd
script:
- cd ./ardupilot/ArduCopter && make configure && make && make px4-v2
- cd ../ArduPlane && make configure && make && make px4-v2
- cd ../APMrover2 && make configure && make && make px4-v2
- cd ../ArduCopter && make configure && make && make vrubrain-v51
- Tools/scripts/build_all_travis.sh
notifications:
webhooks:
urls:
- https://webhooks.gitter.im/e/e5e0b55e353e53945b4b
on_success: change # options: [always|never|change] default: always
on_failure: always # options: [always|never|change] default: always
on_start: false # default: false
APMrover2/APMrover2.pde
View file @ 62e7778b
......@@ -112,6 +112,7 @@
#include <AP_Notify.h> // Notify library
#include <AP_BattMonitor.h> // Battery monitor library
#include <AP_OpticalFlow.h> // Optical Flow library
// Configuration
#include "config.h"
......@@ -200,21 +201,7 @@ static AP_GPS gps;
// flight modes convenience array
static AP_Int8 *modes = &g.mode1;
#if CONFIG_BARO == HAL_BARO_BMP085
static AP_Baro_BMP085 barometer;
#elif CONFIG_BARO == HAL_BARO_PX4
static AP_Baro_PX4 barometer;
#elif CONFIG_BARO == HAL_BARO_VRBRAIN
static AP_Baro_VRBRAIN barometer;
#elif CONFIG_BARO == HAL_BARO_HIL
static AP_Baro_HIL barometer;
#elif CONFIG_BARO == HAL_BARO_MS5611
static AP_Baro_MS5611 barometer(&AP_Baro_MS5611::i2c);
#elif CONFIG_BARO == HAL_BARO_MS5611_SPI
static AP_Baro_MS5611 barometer(&AP_Baro_MS5611::spi);
#else
#error Unrecognized CONFIG_BARO setting
#endif
static AP_Baro barometer;
#if CONFIG_COMPASS == HAL_COMPASS_PX4
static AP_Compass_PX4 compass;
......@@ -224,6 +211,8 @@ static AP_Compass_VRBRAIN compass;
static AP_Compass_HMC5843 compass;
#elif CONFIG_COMPASS == HAL_COMPASS_HIL
static AP_Compass_HIL compass;
#elif CONFIG_COMPASS == HAL_COMPASS_AK8963
static AP_Compass_AK8963_MPU9250 compass;
#else
#error Unrecognized CONFIG_COMPASS setting
#endif
......@@ -258,6 +247,8 @@ static bool verify_command(const AP_Mission::Mission_Command& cmd);
static void exit_mission();
AP_Mission mission(ahrs, &start_command, &verify_command, &exit_mission);
static OpticalFlow optflow;
#if CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL
SITL sitl;
#endif
......@@ -548,6 +539,7 @@ void setup() {
// rover does not use arming nor pre-arm checks
AP_Notify::flags.armed = true;
AP_Notify::flags.pre_arm_check = true;
AP_Notify::flags.pre_arm_gps_check = true;
AP_Notify::flags.failsafe_battery = false;
notify.init(false);
......@@ -633,7 +625,7 @@ static void mount_update(void)
static void update_alt()
{
barometer.read();
barometer.update();
if (should_log(MASK_LOG_IMU)) {
Log_Write_Baro();
}
......
APMrover2/GCS_Mavlink.pde
View file @ 62e7778b
......@@ -177,7 +177,7 @@ static NOINLINE void send_extended_status1(mavlink_channel_t chan)
int16_t battery_current = -1;
int8_t battery_remaining = -1;
if (battery.monitoring() == AP_BATT_MONITOR_VOLTAGE_AND_CURRENT) {
if (battery.has_current()) {
battery_remaining = battery.capacity_remaining_pct();
battery_current = battery.current_amps() * 100;
}
......@@ -530,8 +530,21 @@ bool GCS_MAVLINK::try_send_message(enum ap_message id)
// unused
break;
case MSG_BATTERY2:
CHECK_PAYLOAD_SIZE(BATTERY2);
gcs[chan-MAVLINK_COMM_0].send_battery2(battery);
break;
case MSG_CAMERA_FEEDBACK:
#if CAMERA == ENABLED
CHECK_PAYLOAD_SIZE(CAMERA_FEEDBACK);
camera.send_feedback(chan, gps, ahrs, current_loc);
#endif
break;
case MSG_RETRY_DEFERRED:
case MSG_TERRAIN:
case MSG_OPTICAL_FLOW:
break; // just here to prevent a warning
}
......@@ -753,6 +766,7 @@ GCS_MAVLINK::data_stream_send(void)
send_message(MSG_HWSTATUS);
send_message(MSG_RANGEFINDER);
send_message(MSG_SYSTEM_TIME);
send_message(MSG_BATTERY2);
send_message(MSG_MOUNT_STATUS);
}
}
......@@ -789,7 +803,6 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
// decode
mavlink_command_long_t packet;
mavlink_msg_command_long_decode(msg, &packet);
if (mavlink_check_target(packet.target_system, packet.target_component)) break;
uint8_t result = MAV_RESULT_UNSUPPORTED;
......@@ -1019,10 +1032,6 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
int16_t v[8];
mavlink_msg_rc_channels_override_decode(msg, &packet);
// exit immediately if this command is not meant for this vehicle
if (mavlink_check_target(packet.target_system,packet.target_component))
break;
v[0] = packet.chan1_raw;
v[1] = packet.chan2_raw;
v[2] = packet.chan3_raw;
......@@ -1143,19 +1152,6 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
break;
#endif
default:
// forward unknown messages to the other link if there is one
for (uint8_t i=0; i<num_gcs; i++) {
if (gcs[i].initialised && i != (uint8_t)chan) {
mavlink_channel_t out_chan = (mavlink_channel_t)i;
// only forward if it would fit in the transmit buffer
if (comm_get_txspace(out_chan) > ((uint16_t)msg->len) + MAVLINK_NUM_NON_PAYLOAD_BYTES) {
_mavlink_resend_uart(out_chan, msg);
}
}
}
break;
} // end switch
} // end handle mavlink
......
APMrover2/Log.pde
View file @ 62e7778b
......@@ -319,7 +319,11 @@ static void Log_Write_Attitude()
};
DataFlash.WriteBlock(&pkt, sizeof(pkt));
#if AP_AHRS_NAVEKF_AVAILABLE
DataFlash.Log_Write_EKF(ahrs);
#if OPTFLOW == ENABLED
DataFlash.Log_Write_EKF(ahrs,optflow.enabled());
#else
DataFlash.Log_Write_EKF(ahrs,false);
#endif
DataFlash.Log_Write_AHRS2(ahrs);
#endif
}
......
APMrover2/Parameters.pde
View file @ 62e7778b
......@@ -525,7 +525,7 @@ const AP_Param::Info var_info[] PROGMEM = {
// @Group: BATT_
// @Path: ../libraries/AP_BattMonitor/AP_BattMonitor.cpp
GOBJECT(battery, "BATT_", AP_BattMonitor),
GOBJECT(battery, "BATT", AP_BattMonitor),
// @Group: BRD_
// @Path: ../libraries/AP_BoardConfig/AP_BoardConfig.cpp
......
APMrover2/commands_logic.pde
View file @ 62e7778b
......@@ -308,6 +308,7 @@ static void do_take_picture()
{
#if CAMERA == ENABLED
camera.trigger_pic();
gcs_send_message(MSG_CAMERA_FEEDBACK);
if (should_log(MASK_LOG_CAMERA)) {
DataFlash.Log_Write_Camera(ahrs, gps, current_loc);
}
......
APMrover2/radio.pde
View file @ 62e7778b
......@@ -12,6 +12,10 @@ static void set_control_channels(void)
// set rc channel ranges
channel_steer->set_angle(SERVO_MAX);
channel_throttle->set_angle(100);
// setup correct scaling for ESCs like the UAVCAN PX4ESC which
// take a proportion of speed.
hal.rcout->set_esc_scaling(channel_throttle->radio_min, channel_throttle->radio_max);
}
static void init_rc_in()
......
APMrover2/setup.pde
View file @ 62e7778b
......@@ -341,9 +341,7 @@ setup_flightmodes(uint8_t argc, const Menu::arg *argv)
mode != AUTO &&
mode != RTL)
{
if (mode < MANUAL)
mode = RTL;
else if (mode > RTL)
if (mode > RTL)
mode = MANUAL;
else
mode += radioInputSwitch;
......@@ -467,9 +465,13 @@ static void report_batt_monitor()
//print_blanks(2);
cliSerial->printf_P(PSTR("Batt Mointor\n"));
print_divider();
if(battery.monitoring() == AP_BATT_MONITOR_DISABLED) cliSerial->printf_P(PSTR("Batt monitoring disabled"));
if(battery.monitoring() == AP_BATT_MONITOR_VOLTAGE_ONLY) cliSerial->printf_P(PSTR("Monitoring batt volts"));
if(battery.monitoring() == AP_BATT_MONITOR_VOLTAGE_AND_CURRENT) cliSerial->printf_P(PSTR("Monitoring volts and current"));
if (battery.num_instances() == 0) {
cliSerial->printf_P(PSTR("Batt monitoring disabled"));
} else if (!battery.has_current()) {
cliSerial->printf_P(PSTR("Monitoring batt volts"));
} else {
cliSerial->printf_P(PSTR("Monitoring volts and current"));
}
print_blanks(2);
}
static void report_radio()
......
APMrover2/system.pde
View file @ 62e7778b
......@@ -189,9 +189,6 @@ static void init_ardupilot()
// Do GPS init
gps.init(&DataFlash);
//mavlink_system.sysid = MAV_SYSTEM_ID; // Using g.sysid_this_mav
mavlink_system.compid = 1; //MAV_COMP_ID_IMU; // We do not check for comp id
rc_override_active = hal.rcin->set_overrides(rc_override, 8);
init_rc_in(); // sets up rc channels from radio
......
AntennaTracker/AntennaTracker.pde
View file @ 62e7778b
......@@ -62,6 +62,7 @@
#include <AP_Airspeed.h>
#include <RC_Channel.h>
#include <AP_BoardConfig.h>
#include <AP_OpticalFlow.h>
// Configuration
#include "config.h"
......@@ -116,21 +117,7 @@ void gcs_send_text_fmt(const prog_char_t *fmt, ...);
////////////////////////////////////////////////////////////////////////////////
static AP_GPS gps;
#if CONFIG_BARO == HAL_BARO_BMP085
static AP_Baro_BMP085 barometer;
#elif CONFIG_BARO == HAL_BARO_PX4
static AP_Baro_PX4 barometer;
#elif CONFIG_BARO == HAL_BARO_VRBRAIN
static AP_Baro_VRBRAIN barometer;
#elif CONFIG_BARO == HAL_BARO_HIL
static AP_Baro_HIL barometer;
#elif CONFIG_BARO == HAL_BARO_MS5611
static AP_Baro_MS5611 barometer(&AP_Baro_MS5611::i2c);
#elif CONFIG_BARO == HAL_BARO_MS5611_SPI
static AP_Baro_MS5611 barometer(&AP_Baro_MS5611::spi);
#else
#error Unrecognized CONFIG_BARO setting
#endif
static AP_Baro barometer;
#if CONFIG_COMPASS == HAL_COMPASS_PX4
static AP_Compass_PX4 compass;
......@@ -140,6 +127,8 @@ static AP_Compass_VRBRAIN compass;
static AP_Compass_HMC5843 compass;
#elif CONFIG_COMPASS == HAL_COMPASS_HIL
static AP_Compass_HIL compass;
#elif CONFIG_COMPASS == HAL_COMPASS_AK8963
static AP_Compass_AK8963_MPU9250 compass;
#else
#error Unrecognized CONFIG_COMPASS setting
#endif
......@@ -173,10 +162,6 @@ static RC_Channel channel_pitch(CH_PITCH);
static const uint8_t num_gcs = MAVLINK_COMM_NUM_BUFFERS;
static GCS_MAVLINK gcs[MAVLINK_COMM_NUM_BUFFERS];
// If proxy_mode is enabled, uartC is connected to a remote vehicle,
// not gcs[1]
static GCS_MAVLINK proxy_vehicle;
// board specific config
static AP_BoardConfig BoardConfig;
......@@ -267,6 +252,7 @@ void setup()
// antenna tracker does not use pre-arm checks or battery failsafe
AP_Notify::flags.pre_arm_check = true;
AP_Notify::flags.pre_arm_gps_check = true;
AP_Notify::flags.failsafe_battery = false;
notify.init(false);
......
AntennaTracker/GCS_Mavlink.pde
View file @ 62e7778b
......@@ -259,6 +259,11 @@ bool GCS_MAVLINK::try_send_message(enum ap_message id)
case MSG_FENCE_STATUS:
case MSG_WIND:
case MSG_RANGEFINDER:
case MSG_TERRAIN:
case MSG_BATTERY2:
case MSG_CAMERA_FEEDBACK:
case MSG_MOUNT_STATUS:
case MSG_OPTICAL_FLOW:
break; // just here to prevent a warning
}
return true;
......@@ -437,69 +442,41 @@ GCS_MAVLINK::data_stream_send(void)
}
}
void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
/*
We eavesdrop on MAVLINK_MSG_ID_GLOBAL_POSITION_INT and
MAVLINK_MSG_ID_SCALED_PRESSUREs
*/
void mavlink_snoop(const mavlink_message_t* msg)
{
// in proxy mode all messages from the vehicle are proxied to GCS
if (g.proxy_mode == true && chan == proxy_vehicle.chan) {
// We also eavesdrop on MAVLINK_MSG_ID_GLOBAL_POSITION_INT and MAVLINK_MSG_ID_SCALED_PRESSUREs
switch (msg->msgid) {
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;
}
case MAVLINK_MSG_ID_SCALED_PRESSURE:
{
// decode
mavlink_scaled_pressure_t packet;
mavlink_msg_scaled_pressure_decode(msg, &packet);
tracking_update_pressure(packet);
break;
}
}
// Proxy to all the GCS stations
for (uint8_t i=0; i<num_gcs; i++) {
if (gcs[i].initialised) {
mavlink_channel_t out_chan = (mavlink_channel_t)i;
// only forward if it would fit in the transmit buffer
if (comm_get_txspace(out_chan) > ((uint16_t)msg->len) + MAVLINK_NUM_NON_PAYLOAD_BYTES) {
_mavlink_resend_uart(out_chan, msg);
}
}
}
// no further processing of this messages
return;
switch (msg->msgid) {
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;
}
case MAVLINK_MSG_ID_SCALED_PRESSURE:
{
// decode
mavlink_scaled_pressure_t packet;
mavlink_msg_scaled_pressure_decode(msg, &packet);
tracking_update_pressure(packet);
break;
}
}
}
void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
{
switch (msg->msgid) {
// If we are currently operating as a proxy for a remote,
// alas we have to look inside each packet to see if its for us or for the remote
case MAVLINK_MSG_ID_REQUEST_DATA_STREAM:
{
if (g.proxy_mode == true && proxy_vehicle.initialised)
{
// See if its for the remote
mavlink_request_data_stream_t packet;
mavlink_msg_request_data_stream_decode(msg, &packet);
if (mavlink_check_target(packet.target_system, packet.target_component))
{
// Not for us, must be for the remote
if (comm_get_txspace(proxy_vehicle.chan) > ((uint16_t)msg->len) + MAVLINK_NUM_NON_PAYLOAD_BYTES)
_mavlink_resend_uart(proxy_vehicle.chan, msg);
break;
}
// Else its for us, the tracker
}
handle_request_data_stream(msg, true);
break;
}
......@@ -507,92 +484,30 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
case MAVLINK_MSG_ID_PARAM_REQUEST_LIST:
{
if (g.proxy_mode == true && proxy_vehicle.initialised)
{
// See if its for the remote
mavlink_param_request_list_t packet;
mavlink_msg_param_request_list_decode(msg, &packet);
if (mavlink_check_target(packet.target_system, packet.target_component))
{
// Not for us, must be for the remote
if (comm_get_txspace(proxy_vehicle.chan) > ((uint16_t)msg->len) + MAVLINK_NUM_NON_PAYLOAD_BYTES)
_mavlink_resend_uart(proxy_vehicle.chan, msg);
break;
}
// Else its for us, the tracker
}
handle_param_request_list(msg);
break;
}
case MAVLINK_MSG_ID_PARAM_REQUEST_READ:
{
if (g.proxy_mode == true && proxy_vehicle.initialised)
{
// See if its for the remote
mavlink_param_request_read_t packet;
mavlink_msg_param_request_read_decode(msg, &packet);
if (mavlink_check_target(packet.target_system, packet.target_component))
{
// Not for us, must be for the remote
if (comm_get_txspace(proxy_vehicle.chan) > ((uint16_t)msg->len) + MAVLINK_NUM_NON_PAYLOAD_BYTES)
_mavlink_resend_uart(proxy_vehicle.chan, msg);
break;
}
// Else its for us, the tracker
}
handle_param_request_read(msg);
break;
}
case MAVLINK_MSG_ID_PARAM_SET:
{
if (g.proxy_mode == true && proxy_vehicle.initialised)
{
// See if its for the remote
mavlink_param_set_t packet;
mavlink_msg_param_set_decode(msg, &packet);
if (mavlink_check_target(packet.target_system, packet.target_component))
{
// Not for us, must be for the remote
if (comm_get_txspace(proxy_vehicle.chan) > ((uint16_t)msg->len) + MAVLINK_NUM_NON_PAYLOAD_BYTES)
_mavlink_resend_uart(proxy_vehicle.chan, msg);
break;
}
// Else its for us, the tracker
}
handle_param_set(msg, NULL);
break;
}
case MAVLINK_MSG_ID_HEARTBEAT:
{
// Heartbeats are always proxied to the remote and also handled locally
if (g.proxy_mode == true && proxy_vehicle.initialised)
{
if (comm_get_txspace(proxy_vehicle.chan) > ((uint16_t)msg->len) + MAVLINK_NUM_NON_PAYLOAD_BYTES)
_mavlink_resend_uart(proxy_vehicle.chan, msg);
}
if (msg->sysid != g.sysid_my_gcs) break;
break;
}
case MAVLINK_MSG_ID_COMMAND_LONG:
{
// decode
mavlink_command_long_t packet;
mavlink_msg_command_long_decode(msg, &packet);
if (mavlink_check_target(packet.target_system, packet.target_component)) {
// Its for the remote, proxy it
if (g.proxy_mode == true && proxy_vehicle.initialised) {
if (comm_get_txspace(proxy_vehicle.chan) > ((uint16_t)msg->len) + MAVLINK_NUM_NON_PAYLOAD_BYTES)
_mavlink_resend_uart(proxy_vehicle.chan, msg);
}
break;
}
uint8_t result = MAV_RESULT_UNSUPPORTED;
......@@ -696,14 +611,6 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
// decode
mavlink_mission_write_partial_list_t packet;
mavlink_msg_mission_write_partial_list_decode(msg, &packet);
if (mavlink_check_target(packet.target_system,packet.target_component)) {
if (g.proxy_mode == true && proxy_vehicle.initialised) {
// Its for the remote, proxy it
if (comm_get_txspace(proxy_vehicle.chan) > ((uint16_t)msg->len) + MAVLINK_NUM_NON_PAYLOAD_BYTES)
_mavlink_resend_uart(proxy_vehicle.chan, msg);
}
break;
}
if (packet.start_index == 0)
{
// New home at wp index 0. Ask for it
......@@ -724,14 +631,6 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
uint8_t result = MAV_MISSION_ACCEPTED;
mavlink_msg_mission_item_decode(msg, &packet);
if (mavlink_check_target(packet.target_system,packet.target_component)) {
if (g.proxy_mode == true && proxy_vehicle.initialised) {
// Its for the remote, proxy it
if (comm_get_txspace(proxy_vehicle.chan) > ((uint16_t)msg->len) + MAVLINK_NUM_NON_PAYLOAD_BYTES)
_mavlink_resend_uart(proxy_vehicle.chan, msg);
}
break;
}
struct Location tell_command = {};
......@@ -814,16 +713,6 @@ mission_failed:
{
mavlink_manual_control_t packet;
mavlink_msg_manual_control_decode(msg, &packet);
// if the packet is not for us, send onto the vehicle
if (mavlink_check_target(packet.target,0)) {
if (g.proxy_mode == true && proxy_vehicle.initialised) {
if (comm_get_txspace(proxy_vehicle.chan) > ((uint16_t)msg->len) + MAVLINK_NUM_NON_PAYLOAD_BYTES)
_mavlink_resend_uart(proxy_vehicle.chan, msg);
}
break;
}
if(msg->sysid != g.sysid_my_gcs) break; // Only accept control from our gcs
tracking_manual_control(packet);
break;
}
......@@ -858,14 +747,6 @@ mission_failed:
break;
#endif
default:
// Proxy all other messages to the remote
if (g.proxy_mode && proxy_vehicle.initialised) {
if (comm_get_txspace(proxy_vehicle.chan) > ((uint16_t)msg->len) + MAVLINK_NUM_NON_PAYLOAD_BYTES)
_mavlink_resend_uart(proxy_vehicle.chan, msg);
}
break;
} // end switch
} // end handle mavlink
......@@ -936,10 +817,6 @@ static void gcs_update(void)
gcs[i].update(NULL);
}
}
// Also check for messages from the remote if we are in proxy mode
if (g.proxy_mode == true && proxy_vehicle.initialised) {
proxy_vehicle.update(NULL);
}
}
static void gcs_send_text_P(gcs_severity severity, const prog_char_t *str)
......
AntennaTracker/Parameters.h
View file @ 62e7778b
......@@ -80,7 +80,7 @@ public:
k_param_BoardConfig,
k_param_gps,
k_param_scan_speed,
k_param_proxy_mode,
k_param_proxy_mode_unused, // deprecated
k_param_servo_type,
k_param_onoff_yaw_rate,
k_param_onoff_pitch_rate,
......@@ -126,7 +126,6 @@ public:
AP_Float start_longitude;
AP_Float startup_delay;
AP_Int8 proxy_mode;
AP_Int8 servo_type;
AP_Float onoff_yaw_rate;
AP_Float onoff_pitch_rate;
......
AntennaTracker/Parameters.pde
View file @ 62e7778b
......@@ -122,13 +122,6 @@ const AP_Param::Info var_info[] PROGMEM = {
// @User: Standard
GSCALAR(startup_delay, "STARTUP_DELAY", 0),
// @Param: PROXY_MODE
// @DisplayName: Also act as a MAVLink proxy for a vehicle
// @Description: If true, the tracker will act as a MAVlink proxy for a remote vehicle, and will eavesdrop vehicle position reports.
// @Values: 0:Off,1:On
// @User: Standard
GSCALAR(proxy_mode, "PROXY_MODE", 0),
// @Param: SERVO_TYPE
// @DisplayName: Type of servo system being used
// @Description: This allows selection of position servos or on/off servos
......
AntennaTracker/sensors.pde
View file @ 62e7778b
......@@ -10,7 +10,7 @@ static void init_barometer(void)
// read the barometer and return the updated altitude in meters
static void update_barometer(void)
{
barometer.read();
barometer.update();
}
......
AntennaTracker/system.pde
View file @ 62e7778b
......@@ -28,6 +28,9 @@ static void init_tracker()
// init the GCS
gcs[0].init(hal.uartA);
// set up snooping on other mavlink destinations
gcs[0].set_snoop(mavlink_snoop);
// 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);
......@@ -39,11 +42,7 @@ static void init_tracker()
// we have a 2nd serial port for telemetry
hal.uartC->begin(map_baudrate(g.serial1_baud), 128, SERIAL1_BUFSIZE);
if (g.proxy_mode == true) {
proxy_vehicle.setup_uart(hal.uartC, map_baudrate(g.serial1_baud), 128, SERIAL1_BUFSIZE);
} else {
gcs[1].setup_uart(hal.uartC, map_baudrate(g.serial1_baud), 128, SERIAL1_BUFSIZE);
}
gcs[1].setup_uart(hal.uartC, map_baudrate(g.serial1_baud), 128, SERIAL1_BUFSIZE);
mavlink_system.sysid = g.sysid_this_mav;
......@@ -59,8 +58,6 @@ static void init_tracker()
// GPS Initialization
gps.init(NULL);
mavlink_system.compid = 4;
ahrs.init();
ahrs.set_fly_forward(false);
......
ArduCopter/APM_Config.h
View file @ 62e7778b
......@@ -33,14 +33,14 @@
//#define EPM_ENABLED ENABLED // enable epm cargo gripper costs 500bytes of flash
//#define CLI_ENABLED ENABLED // enable the CLI (command-line-interface) at a cost of 21K of flash space
//#define NAV_GUIDED ENABLED // enable external navigation computer to control vehicle through MAV_CMD_NAV_GUIDED mission commands
//#define OPTFLOW ENABLED // enable optical flow sensor and OF_LOITER flight mode at a cost of 5K of flash space
//#define OPTFLOW ENABLED // enable optical flow sensor at a cost of 5K of flash space
// features below are disabled by default on all boards
//#define SPRAYER ENABLED // enable the crop sprayer feature (two ESC controlled pumps the speed of which depends upon the vehicle's horizontal velocity)
// other settings
//#define THROTTLE_IN_DEADBAND 100 // redefine size of throttle deadband in pwm (0 ~ 1000)
//#define LAND_REQUIRE_MIN_THROTTLE_TO_DISARM DISABLED // when set to DISABLED vehicle will disarm after landing (in LAND mode or RTL) even if pilot has not put throttle to zero
//#define LAND_REQUIRE_MIN_THROTTLE_TO_DISARM ENABLED // when set to ENABLED vehicle will only disarm after landing (in AUTO, LAND or RTL) if pilot has put throttle to zero
//#define HIL_MODE HIL_MODE_SENSORS // build for hardware-in-the-loop simulation
......
ArduCopter/ArduCopter.pde
View file @ 62e7778b
......@@ -155,6 +155,7 @@
#if PARACHUTE == ENABLED
#include <AP_Parachute.h> // Parachute release library
#endif
#include <AP_LandingGear.h> // Landing Gear library
#include <AP_Terrain.h>
// AP_HAL to Arduino compatibility layer
......@@ -258,21 +259,8 @@ static GPS_Glitch gps_glitch(gps);
// flight modes convenience array
static AP_Int8 *flight_modes = &g.flight_mode1;
#if CONFIG_BARO == HAL_BARO_BMP085
static AP_Baro_BMP085 barometer;
#elif CONFIG_BARO == HAL_BARO_PX4
static AP_Baro_PX4 barometer;
#elif CONFIG_BARO == HAL_BARO_VRBRAIN
static AP_Baro_VRBRAIN barometer;
#elif CONFIG_BARO == HAL_BARO_HIL
static AP_Baro_HIL barometer;
#elif CONFIG_BARO == HAL_BARO_MS5611
static AP_Baro_MS5611 barometer(&AP_Baro_MS5611::i2c);
#elif CONFIG_BARO == HAL_BARO_MS5611_SPI
static AP_Baro_MS5611 barometer(&AP_Baro_MS5611::spi);
#else
#error Unrecognized CONFIG_BARO setting
#endif
static AP_Baro barometer;
static Baro_Glitch baro_glitch(barometer);
#if CONFIG_COMPASS == HAL_COMPASS_PX4
......@@ -283,6 +271,8 @@ static AP_Compass_VRBRAIN compass;
static AP_Compass_HMC5843 compass;
#elif CONFIG_COMPASS == HAL_COMPASS_HIL
static AP_Compass_HIL compass;
#elif CONFIG_COMPASS == HAL_COMPASS_AK8963
static AP_Compass_AK8963_MPU9250 compass;
#else
#error Unrecognized CONFIG_COMPASS setting
#endif
......@@ -314,13 +304,14 @@ AP_Mission mission(ahrs, &start_command, &verify_command, &exit_mission);
////////////////////////////////////////////////////////////////////////////////
// Optical flow sensor
////////////////////////////////////////////////////////////////////////////////
#if OPTFLOW == ENABLED
#if CONFIG_HAL_BOARD == HAL_BOARD_APM1 || CONFIG_HAL_BOARD == HAL_BOARD_APM2
static AP_OpticalFlow_ADNS3080 optflow(ahrs);
#elif CONFIG_HAL_BOARD == HAL_BOARD_PX4
static AP_OpticalFlow_PX4 optflow(ahrs);
#if OPTFLOW == ENABLED
static OpticalFlow optflow;
#endif
#endif
// gnd speed limit required to observe optical flow sensor limits
static float ekfGndSpdLimit;
// scale factor applied to velocity controller gain to prevent optical flow noise causing excessive angle demand noise
static float ekfNavVelGainScaler;
////////////////////////////////////////////////////////////////////////////////
// GCS selection
......@@ -458,13 +449,6 @@ static MOTOR_CLASS motors(g.rc_1, g.rc_2, g.rc_3, g.rc_4, g.single_servo_1, g.si
static MOTOR_CLASS motors(g.rc_1, g.rc_2, g.rc_3, g.rc_4);
#endif
////////////////////////////////////////////////////////////////////////////////
// PIDs
////////////////////////////////////////////////////////////////////////////////
// This is used to hold radio tuning values for in-flight CH6 tuning
float tuning_value;
////////////////////////////////////////////////////////////////////////////////
// GPS variables
////////////////////////////////////////////////////////////////////////////////
......@@ -577,18 +561,6 @@ static float baro_climbrate; // barometer climbrate in cm/s
// Current location of the copter
static struct Location current_loc;
////////////////////////////////////////////////////////////////////////////////
// Navigation Roll/Pitch functions
////////////////////////////////////////////////////////////////////////////////
#if OPTFLOW == ENABLED
// The Commanded ROll from the autopilot based on optical flow sensor.
static int32_t of_roll;
// The Commanded pitch from the autopilot based on optical flow sensor. negative Pitch means go forward.
static int32_t of_pitch;
#endif // OPTFLOW == ENABLED
////////////////////////////////////////////////////////////////////////////////
// Throttle integrator
////////////////////////////////////////////////////////////////////////////////
......@@ -697,11 +669,6 @@ static AP_HAL::AnalogSource* rssi_analog_source;
static AP_Mount camera_mount(&current_loc, ahrs, 0);
#endif
#if MOUNT2 == ENABLED
// current_loc uses the baro/gps soloution for altitude rather than gps only.
static AP_Mount camera_mount2(&current_loc, ahrs, 1);
#endif
////////////////////////////////////////////////////////////////////////////////
// AC_Fence library to reduce fly-aways
////////////////////////////////////////////////////////////////////////////////
......@@ -737,6 +704,11 @@ static AP_EPM epm;
static AP_Parachute parachute(relay);
#endif
////////////////////////////////////////////////////////////////////////////////
// Landing Gear Controller
////////////////////////////////////////////////////////////////////////////////
static AP_LandingGear landinggear;
////////////////////////////////////////////////////////////////////////////////
// terrain handling
#if AP_TERRAIN_AVAILABLE
......@@ -777,14 +749,14 @@ static const AP_Scheduler::Task scheduler_tasks[] PROGMEM = {
{ throttle_loop, 8, 45 },
{ update_GPS, 8, 90 },
#if OPTFLOW == ENABLED
{ update_optflow, 8, 20 },
{ update_optical_flow, 2, 20 },
#endif
{ update_batt_compass, 40, 72 },
{ read_aux_switches, 40, 5 },
{ arm_motors_check, 40, 1 },
{ auto_trim, 40, 14 },
{ update_altitude, 40, 100 },
{ run_nav_updates, 40, 80 },
{ run_nav_updates, 8, 80 },
{ update_thr_cruise, 40, 10 },
{ three_hz_loop, 133, 9 },
{ compass_accumulate, 8, 42 },
......@@ -796,6 +768,7 @@ static const AP_Scheduler::Task scheduler_tasks[] PROGMEM = {
{ one_hz_loop, 400, 42 },
{ ekf_dcm_check, 40, 2 },
{ crash_check, 40, 2 },
{ landinggear_update, 40, 1 },
{ gcs_check_input, 8, 550 },
{ gcs_send_heartbeat, 400, 150 },
{ gcs_send_deferred, 8, 720 },
......@@ -851,14 +824,14 @@ static const AP_Scheduler::Task scheduler_tasks[] PROGMEM = {
{ throttle_loop, 2, 450 },
{ update_GPS, 2, 900 },
#if OPTFLOW == ENABLED
{ update_optflow, 2, 100 },
{ update_optical_flow, 1, 100 },
#endif
{ update_batt_compass, 10, 720 },
{ read_aux_switches, 10, 50 },
{ arm_motors_check, 10, 10 },
{ auto_trim, 10, 140 },
{ update_altitude, 10, 1000 },
{ run_nav_updates, 10, 800 },
{ run_nav_updates, 4, 800 },
{ update_thr_cruise, 1, 50 },
{ three_hz_loop, 33, 90 },
{ compass_accumulate, 2, 420 },
......@@ -870,6 +843,7 @@ static const AP_Scheduler::Task scheduler_tasks[] PROGMEM = {
{ one_hz_loop, 100, 420 },
{ ekf_dcm_check, 10, 20 },
{ crash_check, 10, 20 },
{ landinggear_update, 10, 10 },
{ gcs_check_input, 2, 550 },
{ gcs_send_heartbeat, 100, 150 },
{ gcs_send_deferred, 2, 720 },
......@@ -1054,11 +1028,6 @@ static void update_mount()
camera_mount.update_mount_position();
#endif
#if MOUNT2 == ENABLED
// update camera mount's position
camera_mount2.update_mount_position();
#endif
#if CAMERA == ENABLED
camera.trigger_pic_cleanup();
#endif
......@@ -1140,8 +1109,8 @@ static void three_hz_loop()
update_events();
if(g.radio_tuning > 0)
tuning();
// update ch6 in flight tuning
tuning();
}
// one_hz_loop - runs at 1Hz
......@@ -1179,10 +1148,6 @@ static void one_hz_loop()
camera_mount.update_mount_type();
#endif
#if MOUNT2 == ENABLED
camera_mount2.update_mount_type();
#endif
check_usb_mux();
#if AP_TERRAIN_AVAILABLE
......@@ -1368,212 +1333,5 @@ static void update_altitude()
}
}
static void tuning(){
// exit immediately when radio failsafe is invoked so tuning values are not set to zero
if (failsafe.radio || failsafe.radio_counter != 0) {
return;
}
tuning_value = (float)g.rc_6.control_in / 1000.0f;
g.rc_6.set_range(g.radio_tuning_low,g.radio_tuning_high); // 0 to 1
switch(g.radio_tuning) {
// Roll, Pitch tuning
case CH6_STABILIZE_ROLL_PITCH_KP:
g.p_stabilize_roll.kP(tuning_value);
g.p_stabilize_pitch.kP(tuning_value);
break;
case CH6_RATE_ROLL_PITCH_KP:
g.pid_rate_roll.kP(tuning_value);
g.pid_rate_pitch.kP(tuning_value);
break;
case CH6_RATE_ROLL_PITCH_KI:
g.pid_rate_roll.kI(tuning_value);
g.pid_rate_pitch.kI(tuning_value);
break;
case CH6_RATE_ROLL_PITCH_KD:
g.pid_rate_roll.kD(tuning_value);
g.pid_rate_pitch.kD(tuning_value);
break;
// Yaw tuning
case CH6_STABILIZE_YAW_KP:
g.p_stabilize_yaw.kP(tuning_value);
break;
case CH6_YAW_RATE_KP:
g.pid_rate_yaw.kP(tuning_value);
break;
case CH6_YAW_RATE_KD:
g.pid_rate_yaw.kD(tuning_value);
break;
// Altitude and throttle tuning
case CH6_ALTITUDE_HOLD_KP:
g.p_alt_hold.kP(tuning_value);
break;
case CH6_THROTTLE_RATE_KP:
g.p_throttle_rate.kP(tuning_value);
break;
case CH6_THROTTLE_ACCEL_KP:
g.pid_throttle_accel.kP(tuning_value);
break;
case CH6_THROTTLE_ACCEL_KI:
g.pid_throttle_accel.kI(tuning_value);
break;
case CH6_THROTTLE_ACCEL_KD:
g.pid_throttle_accel.kD(tuning_value);
break;
// Loiter and navigation tuning
case CH6_LOITER_POSITION_KP:
g.p_loiter_pos.kP(tuning_value);
break;
case CH6_LOITER_RATE_KP:
g.pid_loiter_rate_lon.kP(tuning_value);
g.pid_loiter_rate_lat.kP(tuning_value);
break;
case CH6_LOITER_RATE_KI:
g.pid_loiter_rate_lon.kI(tuning_value);
g.pid_loiter_rate_lat.kI(tuning_value);
break;
case CH6_LOITER_RATE_KD:
g.pid_loiter_rate_lon.kD(tuning_value);
g.pid_loiter_rate_lat.kD(tuning_value);
break;
case CH6_WP_SPEED:
// set waypoint navigation horizontal speed to 0 ~ 1000 cm/s
wp_nav.set_speed_xy(g.rc_6.control_in);
break;
// Acro roll pitch gain
case CH6_ACRO_RP_KP:
g.acro_rp_p = tuning_value;
break;
// Acro yaw gain
case CH6_ACRO_YAW_KP:
g.acro_yaw_p = tuning_value;
break;
#if FRAME_CONFIG == HELI_FRAME
case CH6_HELI_EXTERNAL_GYRO:
motors.ext_gyro_gain(g.rc_6.control_in);
break;
case CH6_RATE_PITCH_FF:
g.pid_rate_pitch.ff(tuning_value);
break;
case CH6_RATE_ROLL_FF:
g.pid_rate_roll.ff(tuning_value);
break;
case CH6_RATE_YAW_FF:
g.pid_rate_yaw.ff(tuning_value);
break;
#endif
case CH6_OPTFLOW_KP:
g.pid_optflow_roll.kP(tuning_value);
g.pid_optflow_pitch.kP(tuning_value);
break;
case CH6_OPTFLOW_KI:
g.pid_optflow_roll.kI(tuning_value);
g.pid_optflow_pitch.kI(tuning_value);
break;
case CH6_OPTFLOW_KD:
g.pid_optflow_roll.kD(tuning_value);
g.pid_optflow_pitch.kD(tuning_value);
break;
case CH6_AHRS_YAW_KP:
ahrs._kp_yaw.set(tuning_value);
break;
case CH6_AHRS_KP:
ahrs._kp.set(tuning_value);
break;
case CH6_DECLINATION:
// set declination to +-20degrees
compass.set_declination(ToRad((2.0f * g.rc_6.control_in - g.radio_tuning_high)/100.0f), false); // 2nd parameter is false because we do not want to save to eeprom because this would have a performance impact
break;
case CH6_CIRCLE_RATE:
// set circle rate
circle_nav.set_rate(g.rc_6.control_in/25-20); // allow approximately 45 degree turn rate in either direction
break;
case CH6_SONAR_GAIN:
// set sonar gain
g.sonar_gain.set(tuning_value);
break;
#if 0
// disabled for now - we need accessor functions
case CH6_EKF_VERTICAL_POS:
// EKF's baro vs accel (higher rely on accels more, baro impact is reduced)
ahrs.get_NavEKF()._gpsVertPosNoise = tuning_value;
break;
case CH6_EKF_HORIZONTAL_POS:
// EKF's gps vs accel (higher rely on accels more, gps impact is reduced)
ahrs.get_NavEKF()._gpsHorizPosNoise = tuning_value;
break;
case CH6_EKF_ACCEL_NOISE:
// EKF's accel noise (lower means trust accels more, gps & baro less)
ahrs.get_NavEKF()._accNoise = tuning_value;
break;
#endif
case CH6_RC_FEEL_RP:
// roll-pitch input smoothing
g.rc_feel_rp = g.rc_6.control_in / 10;
break;
case CH6_RATE_PITCH_KP:
g.pid_rate_pitch.kP(tuning_value);
break;
case CH6_RATE_PITCH_KI:
g.pid_rate_pitch.kI(tuning_value);
break;
case CH6_RATE_PITCH_KD:
g.pid_rate_pitch.kD(tuning_value);
break;
case CH6_RATE_ROLL_KP:
g.pid_rate_roll.kP(tuning_value);
break;
case CH6_RATE_ROLL_KI:
g.pid_rate_roll.kI(tuning_value);
break;
case CH6_RATE_ROLL_KD:
g.pid_rate_roll.kD(tuning_value);
break;
}
}
AP_HAL_MAIN();
ArduCopter/GCS_Mavlink.pde
View file @ 62e7778b
......@@ -194,7 +194,7 @@ static NOINLINE void send_extended_status1(mavlink_channel_t chan)
MAV_SYS_STATUS_SENSOR_3D_MAG |
MAV_SYS_STATUS_SENSOR_GPS |
MAV_SYS_STATUS_SENSOR_RC_RECEIVER);
if (barometer.healthy()) {
if (barometer.all_healthy()) {
control_sensors_health |= MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE;
}
if (g.compass_enabled && compass.healthy(0) && ahrs.use_compass()) {
......@@ -203,6 +203,11 @@ static NOINLINE void send_extended_status1(mavlink_channel_t chan)
if (gps.status() > AP_GPS::NO_GPS && (!gps_glitch.glitching()||ap.usb_connected)) {
control_sensors_health |= MAV_SYS_STATUS_SENSOR_GPS;
}
#if OPTFLOW == ENABLED
if (optflow.healthy()) {
control_sensors_health |= MAV_SYS_STATUS_SENSOR_OPTICAL_FLOW;
}
#endif
if (ap.rc_receiver_present && !failsafe.radio) {
control_sensors_health |= MAV_SYS_STATUS_SENSOR_RC_RECEIVER;
}
......@@ -221,7 +226,7 @@ static NOINLINE void send_extended_status1(mavlink_channel_t chan)
int16_t battery_current = -1;
int8_t battery_remaining = -1;
if (battery.monitoring() == AP_BATT_MONITOR_VOLTAGE_AND_CURRENT) {
if (battery.has_current()) {
battery_remaining = battery.capacity_remaining_pct();
battery_current = battery.current_amps() * 100;
}
......@@ -640,6 +645,13 @@ bool GCS_MAVLINK::try_send_message(enum ap_message id)
#endif // MOUNT == ENABLED
break;
case MSG_OPTICAL_FLOW:
#if OPTFLOW == ENABLED
CHECK_PAYLOAD_SIZE(OPTICAL_FLOW);
gcs[chan-MAVLINK_COMM_0].send_opticalflow(ahrs, optflow);
#endif
break;
case MSG_FENCE_STATUS:
case MSG_WIND:
// unused
......@@ -866,6 +878,7 @@ GCS_MAVLINK::data_stream_send(void)
send_message(MSG_TERRAIN);
#endif
send_message(MSG_MOUNT_STATUS);
send_message(MSG_OPTICAL_FLOW);
}
}
......@@ -883,8 +896,6 @@ void GCS_MAVLINK::handle_change_alt_request(AP_Mission::Mission_Command &cmd)
}
// To-Do: update target altitude for loiter or waypoint controller depending upon nav mode
// similar to how do_change_alt works
wp_nav.set_desired_alt(cmd.content.location.alt);
}
void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
......@@ -996,11 +1007,6 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
int16_t v[8];
mavlink_msg_rc_channels_override_decode(msg, &packet);
// exit immediately if this command is not meant for this vehicle
if (mavlink_check_target(packet.target_system,packet.target_component)) {
break;
}
v[0] = packet.chan1_raw;
v[1] = packet.chan2_raw;
v[2] = packet.chan3_raw;
......@@ -1025,11 +1031,6 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
mavlink_command_long_t packet;
mavlink_msg_command_long_decode(msg, &packet);
// exit immediately if this command is not meant for this vehicle
if (mavlink_check_target(packet.target_system, packet.target_component)) {
break;
}
switch(packet.command) {
case MAV_CMD_NAV_TAKEOFF:
......@@ -1310,11 +1311,6 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
mavlink_set_position_target_local_ned_t packet;
mavlink_msg_set_position_target_local_ned_decode(msg, &packet);
// exit immediately if this command is not meant for this vehicle
if (mavlink_check_target(packet.target_system, packet.target_component)) {
break;
}
// exit if vehicle is not in Guided mode or Auto-Guided mode
if ((control_mode != GUIDED) && !(control_mode == AUTO && auto_mode == Auto_NavGuided)) {
break;
......@@ -1332,7 +1328,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
*/
if (!pos_ignore && !vel_ignore && acc_ignore) {
guided_set_destination_spline(Vector3f(packet.x * 100.0f, packet.y * 100.0f, -packet.z * 100.0f), Vector3f(packet.vx * 100.0f, packet.vy * 100.0f, -packet.vz * 100.0f));
guided_set_destination_posvel(Vector3f(packet.x * 100.0f, packet.y * 100.0f, -packet.z * 100.0f), Vector3f(packet.vx * 100.0f, packet.vy * 100.0f, -packet.vz * 100.0f));
} else if (pos_ignore && !vel_ignore && acc_ignore) {
guided_set_velocity(Vector3f(packet.vx * 100.0f, packet.vy * 100.0f, -packet.vz * 100.0f));
} else if (!pos_ignore && vel_ignore && acc_ignore) {
......@@ -1350,11 +1346,6 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
mavlink_set_position_target_global_int_t packet;
mavlink_msg_set_position_target_global_int_decode(msg, &packet);
// exit immediately if this command is not meant for this vehicle
if (mavlink_check_target(packet.target_system, packet.target_component)) {
break;
}
// exit if vehicle is not in Guided mode or Auto-Guided mode
if ((control_mode != GUIDED) && !(control_mode == AUTO && auto_mode == Auto_NavGuided)) {
break;
......@@ -1400,7 +1391,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
}
if (!pos_ignore && !vel_ignore && acc_ignore) {
guided_set_destination_spline(pos_ned, Vector3f(packet.vx * 100.0f, packet.vy * 100.0f, -packet.vz * 100.0f));
guided_set_destination_posvel(pos_ned, Vector3f(packet.vx * 100.0f, packet.vy * 100.0f, -packet.vz * 100.0f));
} else if (pos_ignore && !vel_ignore && acc_ignore) {
guided_set_velocity(Vector3f(packet.vx * 100.0f, packet.vy * 100.0f, -packet.vz * 100.0f));
} else if (!pos_ignore && vel_ignore && acc_ignore) {
......@@ -1518,8 +1509,6 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
case MAVLINK_MSG_ID_RALLY_POINT: {
mavlink_rally_point_t packet;
mavlink_msg_rally_point_decode(msg, &packet);
if (mavlink_check_target(packet.target_system, packet.target_component))
break;
if (packet.idx >= rally.get_rally_total() ||
packet.idx >= rally.get_rally_max()) {
......@@ -1553,8 +1542,6 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
mavlink_rally_fetch_point_t packet;
mavlink_msg_rally_fetch_point_decode(msg, &packet);
if (mavlink_check_target(packet.target_system, packet.target_component))
break;
//send_text_P(SEVERITY_HIGH, PSTR("## getting rally point in GCS_Mavlink.pde 2")); // #### TEMP
......
ArduCopter/Log.pde
View file @ 62e7778b
......@@ -241,11 +241,10 @@ struct PACKED log_Optflow {
LOG_PACKET_HEADER;
uint32_t time_ms;
uint8_t surface_quality;
int16_t raw_x;
int16_t raw_y;
float vel_x;
float vel_y;
float dist;
float flow_x;
float flow_y;
float body_x;
float body_y;
};
// Write an optical flow packet
......@@ -256,17 +255,16 @@ static void Log_Write_Optflow()
if (!optflow.enabled()) {
return;
}
const Vector2i &raw = optflow.raw();
const Vector2f &vel = optflow.velocity();
const Vector2f &flowRate = optflow.flowRate();
const Vector2f &bodyRate = optflow.bodyRate();
struct log_Optflow pkt = {
LOG_PACKET_HEADER_INIT(LOG_OPTFLOW_MSG),
time_ms : hal.scheduler->millis(),
surface_quality : optflow.quality(),
raw_x : raw.x,
raw_y : raw.y,
vel_x : vel.x,
vel_y : vel.y,
dist : optflow.ground_distance_m()
flow_x : flowRate.x,
flow_y : flowRate.y,
body_x : bodyRate.x,
body_y : bodyRate.y
};
DataFlash.WriteBlock(&pkt, sizeof(pkt));
#endif // OPTFLOW == ENABLED
......@@ -493,7 +491,11 @@ static void Log_Write_Attitude()
DataFlash.WriteBlock(&pkt, sizeof(pkt));
#if AP_AHRS_NAVEKF_AVAILABLE
DataFlash.Log_Write_EKF(ahrs);
#if OPTFLOW == ENABLED
DataFlash.Log_Write_EKF(ahrs,optflow.enabled());
#else
DataFlash.Log_Write_EKF(ahrs,false);
#endif
DataFlash.Log_Write_AHRS2(ahrs);
#endif
#if CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL
......@@ -676,7 +678,7 @@ static const struct LogStructure log_structure[] PROGMEM = {
{ LOG_CURRENT_MSG, sizeof(log_Current),
"CURR", "IhIhhhf", "TimeMS,ThrOut,ThrInt,Volt,Curr,Vcc,CurrTot" },
{ LOG_OPTFLOW_MSG, sizeof(log_Optflow),
"OF", "IBhhfff", "TimeMS,Qual,RawX,RawY,VelX,VelY,Dist" },
"OF", "IBffff", "TimeMS,Qual,flowX,flowY,bodyX,bodyY" },
{ LOG_NAV_TUNING_MSG, sizeof(log_Nav_Tuning),
"NTUN", "Iffffffffff", "TimeMS,DPosX,DPosY,PosX,PosY,DVelX,DVelY,VelX,VelY,DAccX,DAccY" },
{ LOG_CONTROL_TUNING_MSG, sizeof(log_Control_Tuning),
......