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 (4287)
4,187 additional commits have been omitted to prevent performance issues.
Expand all Hide whitespace changes
Inline Side-by-side
Showing
with 992 additions and 1850 deletions
.editorconfig 0 → 100644
View file @ 62e7778b
# This file provide reference settings for the APM code conventions
# There are plug-ins available for nearly every text editor to automatically
# respect the conventions contained within this file.
#
# Please see editorconfig.org for complete information.
#
# If you find errors in this file, please send a pull-request with a fix.
#
root = true
[*]
indent_style = space
indent_size = 4
end_of_line = lf
charset = utf-8
trim_trailing_whitespace = false # These are the correct rules for APM coding standards, but fixing up old files causes git spam
insert_final_newline = false
[*.mk]
indent_style = tab
indent_size = 8
[{makefile,Makefile}]
indent_style = tab
indent_size = 8
.gitignore
View file @ 62e7778b
......@@ -3,14 +3,18 @@
*.d
*.dll
*.elf
*.hex
*.dfu
*.exe
*.lst
*.o
*.obj
*.px4
*.vrx
*.pyc
*.tlog
*.tlog.raw
*.vbrain
*.zip
.*.swo
.*.swp
......@@ -43,19 +47,30 @@
/Tools/ArdupilotMegaPlanner/wix/bin
/Tools/ArdupilotMegaPlanner/wix/obj
/Tools/autotest/ch7_mission.txt
ArduCopter/dataflash.bin
/Tools/autotest/aircraft/Rascal/reset.xml
/Tools/autotest/jsbsim/fgout.xml
/Tools/autotest/jsbsim/rascal_test.xml
ArduCopter/Debug/
ArduCopter/eeprom.bin
ArduCopter/terrain/*.DAT
ArduCopter/test.ArduCopter/
ArduCopter/test/*
ArduPlane/terrain/*.DAT
ArduPlane/test.ArduPlane/
APMrover2/test.APMrover2/
autotest.lck
build
Build.ArduCopter/*
Build.ArduPlane/*
Build.APMrover2/*
Build.AntennaTracker/*
cmake_install.cmake
CMakeCache.txt
CMakeFiles
config.mk
dataflash.bin
eeprom.bin
index.html
LASTLOG.TXT
Make.dep
mav.log
mav.log.raw
......@@ -64,3 +79,4 @@ serialsent.raw
status.txt
tags
test.ArduCopter/*
Thumbs.db
.pydevproject 0 → 100644
View file @ 62e7778b
<?xml version="1.0" encoding="UTF-8" standalone="no"?>
<?eclipse-pydev version="1.0"?><pydev_project>
<pydev_property name="org.python.pydev.PYTHON_PROJECT_INTERPRETER">Default</pydev_property>
<pydev_property name="org.python.pydev.PYTHON_PROJECT_VERSION">python 2.7</pydev_property>
</pydev_project>
.travis.yml 0 → 100644
View file @ 62e7778b
language: cpp
before_install:
- APMDIR=$(pwd) && pushd .. && $APMDIR/Tools/scripts/install-prereqs-ubuntu.sh -y && . ~/.profile && popd
script:
- 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
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#define THISFIRMWARE "ArduRover v2.45"
#define THISFIRMWARE "ArduRover v2.47"
/*
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
......@@ -62,6 +62,7 @@
#include <AP_HAL.h>
#include <AP_Menu.h>
#include <AP_Param.h>
#include <StorageManager.h>
#include <AP_GPS.h> // ArduPilot GPS library
#include <AP_ADC.h> // ArduPilot Mega Analog to Digital Converter Library
#include <AP_ADC_AnalogSource.h>
......@@ -70,6 +71,10 @@
#include <AP_Math.h> // ArduPilot Mega Vector/Matrix math Library
#include <AP_InertialSensor.h> // Inertial Sensor (uncalibated IMU) Library
#include <AP_AHRS.h> // ArduPilot Mega DCM Library
#include <AP_NavEKF.h>
#include <AP_Mission.h> // Mission command library
#include <AP_Rally.h>
#include <AP_Terrain.h>
#include <PID.h> // PID library
#include <RC_Channel.h> // RC Channel Library
#include <AP_RangeFinder.h> // Range finder library
......@@ -94,10 +99,12 @@
#include <APM_Control.h>
#include <AP_L1_Control.h>
#include <AP_BoardConfig.h>
#include <AP_Frsky_Telem.h>
#include <AP_HAL_AVR.h>
#include <AP_HAL_AVR_SITL.h>
#include <AP_HAL_PX4.h>
#include <AP_HAL_VRBRAIN.h>
#include <AP_HAL_FLYMAPLE.h>
#include <AP_HAL_Linux.h>
#include <AP_HAL_Empty.h>
......@@ -105,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"
......@@ -124,7 +132,7 @@ const AP_HAL::HAL& hal = AP_HAL_BOARD_DRIVER;
// must be the first AP_Param variable declared to ensure its
// constructor runs before the constructors of the other AP_Param
// variables
AP_Param param_loader(var_info, WP_START_BYTE);
AP_Param param_loader(var_info);
////////////////////////////////////////////////////////////////////////////////
// the rate we run the main loop at
......@@ -166,13 +174,8 @@ static void print_mode(AP_HAL::BetterStream *port, uint8_t mode);
static DataFlash_APM1 DataFlash;
#elif CONFIG_HAL_BOARD == HAL_BOARD_APM2
static DataFlash_APM2 DataFlash;
#elif CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL
static DataFlash_File DataFlash("logs");
//static DataFlash_SITL DataFlash;
#elif CONFIG_HAL_BOARD == HAL_BOARD_PX4
static DataFlash_File DataFlash("/fs/microsd/APM/LOGS");
#elif CONFIG_HAL_BOARD == HAL_BOARD_LINUX
static DataFlash_File DataFlash("logs");
#elif defined(HAL_BOARD_LOG_DIRECTORY)
static DataFlash_File DataFlash(HAL_BOARD_LOG_DIRECTORY);
#else
DataFlash_Empty DataFlash;
#endif
......@@ -192,72 +195,40 @@ static bool in_log_download;
// supply data from the simulation.
//
// All GPS access should be through this pointer.
static GPS *g_gps;
// GPS driver
static AP_GPS gps;
// flight modes convenience array
static AP_Int8 *modes = &g.mode1;
#if CONFIG_HAL_BOARD == HAL_BOARD_APM1
static AP_ADC_ADS7844 adc;
#endif
static AP_Baro barometer;
#if CONFIG_COMPASS == AP_COMPASS_PX4
#if CONFIG_COMPASS == HAL_COMPASS_PX4
static AP_Compass_PX4 compass;
#elif CONFIG_COMPASS == AP_COMPASS_HMC5843
#elif CONFIG_COMPASS == HAL_COMPASS_VRBRAIN
static AP_Compass_VRBRAIN compass;
#elif CONFIG_COMPASS == HAL_COMPASS_HMC5843
static AP_Compass_HMC5843 compass;
#elif CONFIG_COMPASS == AP_COMPASS_HIL
#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
// GPS selection
#if GPS_PROTOCOL == GPS_PROTOCOL_AUTO
AP_GPS_Auto g_gps_driver(&g_gps);
#elif GPS_PROTOCOL == GPS_PROTOCOL_NMEA
AP_GPS_NMEA g_gps_driver;
#elif GPS_PROTOCOL == GPS_PROTOCOL_SIRF
AP_GPS_SIRF g_gps_driver;
#elif GPS_PROTOCOL == GPS_PROTOCOL_UBLOX
AP_GPS_UBLOX g_gps_driver;
#elif GPS_PROTOCOL == GPS_PROTOCOL_MTK
AP_GPS_MTK g_gps_driver;
#elif GPS_PROTOCOL == GPS_PROTOCOL_MTK19
AP_GPS_MTK19 g_gps_driver;
#elif GPS_PROTOCOL == GPS_PROTOCOL_NONE
AP_GPS_None g_gps_driver;
#if CONFIG_HAL_BOARD == HAL_BOARD_APM1
AP_ADC_ADS7844 apm1_adc;
#endif
#elif GPS_PROTOCOL == GPS_PROTOCOL_HIL
AP_GPS_HIL g_gps_driver;
AP_InertialSensor ins;
// Inertial Navigation EKF
#if AP_AHRS_NAVEKF_AVAILABLE
AP_AHRS_NavEKF ahrs(ins, barometer, gps);
#else
#error Unrecognised GPS_PROTOCOL setting.
#endif // GPS PROTOCOL
#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_FLYMAPLE
AP_InertialSensor_Flymaple ins;
#elif CONFIG_INS_TYPE == CONFIG_INS_L3G4200D
AP_InertialSensor_L3G4200D ins;
#elif CONFIG_INS_TYPE == CONFIG_INS_OILPAN
AP_InertialSensor_Oilpan ins( &adc );
#else
#error Unrecognised CONFIG_INS_TYPE setting.
#endif // CONFIG_INS_TYPE
AP_AHRS_DCM ahrs(ins, g_gps);
AP_AHRS_DCM ahrs(ins, barometer, gps);
#endif
static AP_L1_Control L1_controller(ahrs);
......@@ -267,6 +238,17 @@ static AP_Navigation *nav_controller = &L1_controller;
// steering controller
static AP_SteerController steerController(ahrs);
////////////////////////////////////////////////////////////////////////////////
// Mission library
// forward declaration to avoid compiler errors
////////////////////////////////////////////////////////////////////////////////
static bool start_command(const AP_Mission::Mission_Command& cmd);
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
......@@ -282,14 +264,9 @@ static GCS_MAVLINK gcs[MAVLINK_COMM_NUM_BUFFERS];
// is to take the 0 to 1024 range down to an 8 bit range for MAVLink
AP_HAL::AnalogSource *rssi_analog_source;
AP_HAL::AnalogSource *vcc_pin;
////////////////////////////////////////////////////////////////////////////////
// SONAR selection
////////////////////////////////////////////////////////////////////////////////
//
static AP_RangeFinder_analog sonar;
static AP_RangeFinder_analog sonar2;
// SONAR
static RangeFinder sonar;
// relay support
AP_Relay relay;
......@@ -309,8 +286,7 @@ static struct Location current_loc;
// --------------------------------------
#if MOUNT == ENABLED
// current_loc uses the baro/gps soloution for altitude rather than gps only.
// mabe one could use current_loc for lat/lon too and eliminate g_gps alltogether?
AP_Mount camera_mount(&current_loc, g_gps, ahrs, 0);
AP_Mount camera_mount(&current_loc, ahrs, 0);
#endif
......@@ -367,23 +343,9 @@ static struct {
// notification object for LEDs, buzzers etc (parameter set to false disables external leds)
static AP_Notify notify;
////////////////////////////////////////////////////////////////////////////////
// GPS variables
////////////////////////////////////////////////////////////////////////////////
// This is used to scale GPS values for EEPROM storage
// 10^7 times Decimal GPS means 1 == 1cm
// This approximation makes calculations integer and it's easy to read
static const float t7 = 10000000.0;
// We use atan2 and other trig techniques to calaculate angles
// A counter used to count down valid gps fixes to allow the gps estimate to settle
// before recording our home position (and executing a ground start if we booted with an air start)
static uint8_t ground_start_count = 5;
// Used to compute a speed estimate from the first valid gps fixes to decide if we are
// on the ground or in the air. Used to decide if a ground start is appropriate if we
// booted with an air start.
static int16_t ground_start_avg;
static int32_t gps_base_alt;
static uint8_t ground_start_count = 20;
////////////////////////////////////////////////////////////////////////////////
// Location & Navigation
......@@ -397,14 +359,9 @@ static bool have_position;
static bool rtl_complete = false;
// There may be two active commands in Auto mode.
// This indicates the active navigation command by index number
static uint8_t nav_command_index;
// This indicates the active non-navigation command by index number
static uint8_t non_nav_command_index;
// This is the command type (eg navigate to waypoint) of the active navigation command
static uint8_t nav_command_ID = NO_COMMAND;
static uint8_t non_nav_command_ID = NO_COMMAND;
// angle of our next navigation waypoint
static int32_t next_navigation_leg_cd;
// ground speed error in m/s
static float groundspeed_error;
......@@ -447,15 +404,19 @@ static int16_t throttle_last = 0, throttle = 500;
// When CH7 goes LOW PWM from HIGH PWM, this value will have been set true
// This allows advanced functionality to know when to execute
static bool ch7_flag;
// This register tracks the current Mission Command index when writing
// a mission using CH7 in flight
static int8_t CH7_wp_index;
////////////////////////////////////////////////////////////////////////////////
// Battery Sensors
////////////////////////////////////////////////////////////////////////////////
static AP_BattMonitor battery;
////////////////////////////////////////////////////////////////////////////////
// Battery Sensors
////////////////////////////////////////////////////////////////////////////////
#if FRSKY_TELEM_ENABLED == ENABLED
static AP_Frsky_Telem frsky_telemetry(ahrs, battery);
#endif
////////////////////////////////////////////////////////////////////////////////
// Navigation control variables
////////////////////////////////////////////////////////////////////////////////
......@@ -479,16 +440,14 @@ static int32_t condition_value;
// A starting value used to check the status of a conditional command.
// For example in a delay command the condition_start records that start time for the delay
static int32_t condition_start;
// A value used in condition commands. For example the rate at which to change altitude.
static int16_t condition_rate;
////////////////////////////////////////////////////////////////////////////////
// 3D Location vectors
// Location structure defined in AP_Common
////////////////////////////////////////////////////////////////////////////////
// The home location used for RTL. The location is set when we first get stable GPS lock
static struct Location home;
// Flag for if we have g_gps lock and have set the home location
static const struct Location &home = ahrs.get_home();
// Flag for if we have gps lock and have set the home location
static bool home_is_set;
// The location of the previous waypoint. Used for track following and altitude ramp calculations
static struct Location prev_WP;
......@@ -497,11 +456,6 @@ static struct Location next_WP;
// The location of the active waypoint in Guided mode.
static struct Location guided_WP;
// The location structure information from the Nav command being processed
static struct Location next_nav_command;
// The location structure information from the Non-Nav command being processed
static struct Location next_nonnav_command;
////////////////////////////////////////////////////////////////////////////////
// IMU variables
////////////////////////////////////////////////////////////////////////////////
......@@ -516,8 +470,6 @@ static float G_Dt = 0.02;
static int32_t perf_mon_timer;
// The maximum main loop execution time recorded in the current performance monitoring interval
static uint32_t G_Dt_max;
// The number of gps fixes recorded in the current performance monitoring interval
static uint8_t gps_fix_count = 0;
////////////////////////////////////////////////////////////////////////////////
// System Timers
......@@ -549,6 +501,7 @@ static const AP_Scheduler::Task scheduler_tasks[] PROGMEM = {
{ set_servos, 1, 1500 },
{ update_GPS_50Hz, 1, 2500 },
{ update_GPS_10Hz, 5, 2500 },
{ update_alt, 5, 3400 },
{ navigate, 5, 1600 },
{ update_compass, 5, 2000 },
{ update_commands, 5, 1000 },
......@@ -567,7 +520,10 @@ static const AP_Scheduler::Task scheduler_tasks[] PROGMEM = {
{ gcs_failsafe_check, 5, 600 },
{ compass_accumulate, 1, 900 },
{ update_notify, 1, 300 },
{ one_second_loop, 50, 3000 }
{ one_second_loop, 50, 3000 },
#if FRSKY_TELEM_ENABLED == ENABLED
{ telemetry_send, 10, 100 }
#endif
};
......@@ -583,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);
......@@ -590,7 +547,6 @@ void setup() {
battery.init();
rssi_analog_source = hal.analogin->channel(ANALOG_INPUT_NONE);
vcc_pin = hal.analogin->channel(ANALOG_INPUT_BOARD_VCC);
init_ardupilot();
......@@ -604,9 +560,8 @@ void setup() {
void loop()
{
// wait for an INS sample
if (!ins.wait_for_sample(1000)) {
return;
}
ins.wait_for_sample();
uint32_t timer = hal.scheduler->micros();
delta_us_fast_loop = timer - fast_loopTimer_us;
......@@ -627,6 +582,8 @@ void loop()
// update AHRS system
static void ahrs_update()
{
ahrs.set_armed(hal.util->safety_switch_state() != AP_HAL::Util::SAFETY_DISARMED);
#if HIL_MODE != HIL_MODE_DISABLED
// update hil before AHRS update
gcs_update();
......@@ -640,6 +597,12 @@ static void ahrs_update()
ahrs.update();
// if using the EKF get a speed update now (from accelerometers)
Vector3f velocity;
if (ahrs.get_velocity_NED(velocity)) {
ground_speed = pythagorous2(velocity.x, velocity.y);
}
if (should_log(MASK_LOG_ATTITUDE_FAST))
Log_Write_Attitude();
......@@ -660,6 +623,14 @@ static void mount_update(void)
#endif
}
static void update_alt()
{
barometer.update();
if (should_log(MASK_LOG_IMU)) {
Log_Write_Baro();
}
}
/*
check for GCS failsafe - 10Hz
*/
......@@ -688,7 +659,7 @@ static void update_compass(void)
if (g.compass_enabled && compass.read()) {
ahrs.set_compass(&compass);
// update offsets
compass.null_offsets();
compass.learn_offsets();
if (should_log(MASK_LOG_COMPASS)) {
Log_Write_Compass();
}
......@@ -733,14 +704,7 @@ static void update_logging2(void)
*/
static void update_aux(void)
{
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
update_aux_servo_function(&g.rc_5, &g.rc_6, &g.rc_7, &g.rc_8, &g.rc_9, &g.rc_10, &g.rc_11, &g.rc_12);
#elif CONFIG_HAL_BOARD == HAL_BOARD_APM2
update_aux_servo_function(&g.rc_5, &g.rc_6, &g.rc_7, &g.rc_8, &g.rc_10, &g.rc_11);
#else
update_aux_servo_function(&g.rc_5, &g.rc_6, &g.rc_7, &g.rc_8);
#endif
enable_aux_servos();
RC_Channel_aux::enable_aux_servos();
#if MOUNT == ENABLED
camera_mount.update_mount_type();
......@@ -798,13 +762,15 @@ static void one_second_loop(void)
static void update_GPS_50Hz(void)
{
static uint32_t last_gps_reading;
g_gps->update();
if (g_gps->last_message_time_ms() != last_gps_reading) {
last_gps_reading = g_gps->last_message_time_ms();
if (should_log(MASK_LOG_GPS)) {
DataFlash.Log_Write_GPS(g_gps, current_loc.alt);
static uint32_t last_gps_reading[GPS_MAX_INSTANCES];
gps.update();
for (uint8_t i=0; i<gps.num_sensors(); i++) {
if (gps.last_message_time_ms(i) != last_gps_reading[i]) {
last_gps_reading[i] = gps.last_message_time_ms(i);
if (should_log(MASK_LOG_GPS)) {
DataFlash.Log_Write_GPS(gps, i, current_loc.alt);
}
}
}
}
......@@ -812,36 +778,38 @@ static void update_GPS_50Hz(void)
static void update_GPS_10Hz(void)
{
have_position = ahrs.get_projected_position(current_loc);
have_position = ahrs.get_position(current_loc);
if (g_gps->new_data && g_gps->status() >= GPS::GPS_OK_FIX_3D) {
gps_fix_count++;
if (have_position && gps.status() >= AP_GPS::GPS_OK_FIX_3D) {
if(ground_start_count > 1){
if (ground_start_count > 1){
ground_start_count--;
ground_start_avg += g_gps->ground_speed_cm;
} else if (ground_start_count == 1) {
// We countdown N number of good GPS fixes
// so that the altitude is more accurate
// -------------------------------------
if (current_loc.lat == 0) {
ground_start_count = 5;
ground_start_count = 20;
} else {
init_home();
// set system clock for log timestamps
hal.util->set_system_clock(g_gps->time_epoch_usec());
hal.util->set_system_clock(gps.time_epoch_usec());
if (g.compass_enabled) {
// Set compass declination automatically
compass.set_initial_location(g_gps->latitude, g_gps->longitude);
compass.set_initial_location(gps.location().lat, gps.location().lng);
}
ground_start_count = 0;
}
}
ground_speed = g_gps->ground_speed_cm * 0.01;
Vector3f velocity;
if (ahrs.get_velocity_NED(velocity)) {
ground_speed = pythagorous2(velocity.x, velocity.y);
} else {
ground_speed = gps.ground_speed();
}
#if CAMERA == ENABLED
if (camera.update_location(current_loc) == true) {
......@@ -931,7 +899,7 @@ static void update_navigation()
break;
case AUTO:
verify_commands();
mission.update();
break;
case RTL:
......
APMrover2/GCS_Mavlink.pde
View file @ 62e7778b
This diff is collapsed.
APMrover2/Log.pde
View file @ 62e7778b
......@@ -169,8 +169,6 @@ struct PACKED log_Performance {
uint32_t loop_time;
uint16_t main_loop_count;
uint32_t g_dt_max;
uint8_t renorm_count;
uint8_t renorm_blowup;
int16_t gyro_drift_x;
int16_t gyro_drift_y;
int16_t gyro_drift_z;
......@@ -187,8 +185,6 @@ static void Log_Write_Performance()
loop_time : millis()- perf_mon_timer,
main_loop_count : mainLoop_count,
g_dt_max : G_Dt_max,
renorm_count : ahrs.renorm_range_count,
renorm_blowup : ahrs.renorm_blowup_count,
gyro_drift_x : (int16_t)(ahrs.get_gyro_drift().x * 1000),
gyro_drift_y : (int16_t)(ahrs.get_gyro_drift().y * 1000),
gyro_drift_z : (int16_t)(ahrs.get_gyro_drift().z * 1000),
......@@ -198,66 +194,12 @@ static void Log_Write_Performance()
DataFlash.WriteBlock(&pkt, sizeof(pkt));
}
struct PACKED log_Cmd {
LOG_PACKET_HEADER;
uint32_t time_ms;
uint8_t command_total;
uint8_t command_number;
uint8_t waypoint_id;
uint8_t waypoint_options;
uint8_t waypoint_param1;
int32_t waypoint_altitude;
int32_t waypoint_latitude;
int32_t waypoint_longitude;
};
// Write a command processing packet. Total length : 19 bytes
static void Log_Write_Cmd(uint8_t num, const struct Location *wp)
// Write a mission command. Total length : 36 bytes
static void Log_Write_Cmd(const AP_Mission::Mission_Command &cmd)
{
struct log_Cmd pkt = {
LOG_PACKET_HEADER_INIT(LOG_CMD_MSG),
time_ms : millis(),
command_total : g.command_total,
command_number : num,
waypoint_id : wp->id,
waypoint_options : wp->options,
waypoint_param1 : wp->p1,
waypoint_altitude : wp->alt,
waypoint_latitude : wp->lat,
waypoint_longitude : wp->lng
};
DataFlash.WriteBlock(&pkt, sizeof(pkt));
}
struct PACKED log_Camera {
LOG_PACKET_HEADER;
uint32_t time_ms;
uint32_t gps_time;
uint16_t gps_week;
int32_t latitude;
int32_t longitude;
int16_t roll;
int16_t pitch;
uint16_t yaw;
};
// Write a Camera packet. Total length : 26 bytes
static void Log_Write_Camera()
{
#if CAMERA == ENABLED
struct log_Camera pkt = {
LOG_PACKET_HEADER_INIT(LOG_CAMERA_MSG),
time_ms : millis(),
gps_time : g_gps->time_week_ms,
gps_week : g_gps->time_week,
latitude : current_loc.lat,
longitude : current_loc.lng,
roll : (int16_t)ahrs.roll_sensor,
pitch : (int16_t)ahrs.pitch_sensor,
yaw : (uint16_t)ahrs.yaw_sensor
};
DataFlash.WriteBlock(&pkt, sizeof(pkt));
#endif
mavlink_mission_item_t mav_cmd = {};
AP_Mission::mission_cmd_to_mavlink(cmd,mav_cmd);
DataFlash.Log_Write_MavCmd(mission.num_commands(),mav_cmd);
}
struct PACKED log_Steering {
......@@ -274,7 +216,7 @@ static void Log_Write_Steering()
LOG_PACKET_HEADER_INIT(LOG_STEERING_MSG),
time_ms : hal.scheduler->millis(),
demanded_accel : lateral_acceleration,
achieved_accel : g_gps->ground_speed_cm * 0.01f * ins.get_gyro().z,
achieved_accel : gps.ground_speed() * ins.get_gyro().z,
};
DataFlash.WriteBlock(&pkt, sizeof(pkt));
}
......@@ -283,7 +225,7 @@ struct PACKED log_Startup {
LOG_PACKET_HEADER;
uint32_t time_ms;
uint8_t startup_type;
uint8_t command_total;
uint16_t command_total;
};
static void Log_Write_Startup(uint8_t type)
......@@ -292,15 +234,16 @@ static void Log_Write_Startup(uint8_t type)
LOG_PACKET_HEADER_INIT(LOG_STARTUP_MSG),
time_ms : millis(),
startup_type : type,
command_total : g.command_total
command_total : mission.num_commands()
};
DataFlash.WriteBlock(&pkt, sizeof(pkt));
// write all commands to the dataflash as well
struct Location cmd;
for (uint8_t i = 0; i <= g.command_total; i++) {
cmd = get_cmd_with_index(i);
Log_Write_Cmd(i, &cmd);
AP_Mission::Mission_Command cmd;
for (uint16_t i = 0; i < mission.num_commands(); i++) {
if(mission.read_cmd_from_storage(i,cmd)) {
Log_Write_Cmd(cmd);
}
}
}
......@@ -375,6 +318,14 @@ static void Log_Write_Attitude()
yaw : (uint16_t)ahrs.yaw_sensor
};
DataFlash.WriteBlock(&pkt, sizeof(pkt));
#if AP_AHRS_NAVEKF_AVAILABLE
#if OPTFLOW == ENABLED
DataFlash.Log_Write_EKF(ahrs,optflow.enabled());
#else
DataFlash.Log_Write_EKF(ahrs,false);
#endif
DataFlash.Log_Write_AHRS2(ahrs);
#endif
}
struct PACKED log_Mode {
......@@ -421,8 +372,8 @@ static void Log_Write_Sonar()
LOG_PACKET_HEADER_INIT(LOG_SONAR_MSG),
time_ms : millis(),
lateral_accel : lateral_acceleration,
sonar1_distance : (uint16_t)sonar.distance_cm(),
sonar2_distance : (uint16_t)sonar2.distance_cm(),
sonar1_distance : (uint16_t)sonar.distance_cm(0),
sonar2_distance : (uint16_t)sonar.distance_cm(1),
detected_count : obstacle.detected_count,
turn_angle : (int8_t)obstacle.turn_angle,
turn_time : turn_time,
......@@ -450,10 +401,13 @@ static void Log_Write_Current()
throttle_in : channel_throttle->control_in,
battery_voltage : (int16_t)(battery.voltage() * 100.0),
current_amps : (int16_t)(battery.current_amps() * 100.0),
board_voltage : board_voltage(),
board_voltage : (uint16_t)(hal.analogin->board_voltage()*1000),
current_total : battery.current_total_mah()
};
DataFlash.WriteBlock(&pkt, sizeof(pkt));
// also write power status
DataFlash.Log_Write_Power();
}
struct PACKED log_Compass {
......@@ -511,6 +465,27 @@ static void Log_Write_Compass()
DataFlash.WriteBlock(&pkt2, sizeof(pkt2));
}
#endif
#if COMPASS_MAX_INSTANCES > 2
if (compass.get_count() > 2) {
const Vector3f &mag3_offsets = compass.get_offsets(2);
const Vector3f &mag3_motor_offsets = compass.get_motor_offsets(2);
const Vector3f &mag3 = compass.get_field(2);
struct log_Compass pkt3 = {
LOG_PACKET_HEADER_INIT(LOG_COMPASS3_MSG),
time_ms : millis(),
mag_x : (int16_t)mag3.x,
mag_y : (int16_t)mag3.y,
mag_z : (int16_t)mag3.z,
offset_x : (int16_t)mag3_offsets.x,
offset_y : (int16_t)mag3_offsets.y,
offset_z : (int16_t)mag3_offsets.z,
motor_offset_x : (int16_t)mag3_motor_offsets.x,
motor_offset_y : (int16_t)mag3_motor_offsets.y,
motor_offset_z : (int16_t)mag3_motor_offsets.z
};
DataFlash.WriteBlock(&pkt3, sizeof(pkt3));
}
#endif
}
......@@ -520,18 +495,19 @@ static void Log_Write_RC(void)
DataFlash.Log_Write_RCOUT();
}
static void Log_Write_Baro(void)
{
DataFlash.Log_Write_Baro(barometer);
}
static const struct LogStructure log_structure[] PROGMEM = {
LOG_COMMON_STRUCTURES,
{ LOG_ATTITUDE_MSG, sizeof(log_Attitude),
"ATT", "IccC", "TimeMS,Roll,Pitch,Yaw" },
{ LOG_PERFORMANCE_MSG, sizeof(log_Performance),
"PM", "IIHIBBhhhBH", "TimeMS,LTime,MLC,gDt,RNCnt,RNBl,GDx,GDy,GDz,I2CErr,INSErr" },
{ LOG_CMD_MSG, sizeof(log_Cmd),
"CMD", "IBBBBBeLL", "TimeMS,CTot,CNum,CId,COpt,Prm1,Alt,Lat,Lng" },
{ LOG_CAMERA_MSG, sizeof(log_Camera),
"CAM", "IIHLLeccC", "TimeMS,GPSTime,GPSWeek,Lat,Lng,Alt,Roll,Pitch,Yaw" },
"PM", "IIHIhhhBH", "TimeMS,LTime,MLC,gDt,GDx,GDy,GDz,I2CErr,INSErr" },
{ LOG_STARTUP_MSG, sizeof(log_Startup),
"STRT", "IBB", "TimeMS,SType,CTot" },
"STRT", "IBH", "TimeMS,SType,CTot" },
{ LOG_CTUN_MSG, sizeof(log_Control_Tuning),
"CTUN", "Ihcchf", "TimeMS,Steer,Roll,Pitch,ThrOut,AccY" },
{ LOG_NTUN_MSG, sizeof(log_Nav_Tuning),
......@@ -588,10 +564,10 @@ static void start_logging()
// dummy functions
static void Log_Write_Startup(uint8_t type) {}
static void Log_Write_Cmd(uint8_t num, const struct Location *wp) {}
static void Log_Write_Current() {}
static void Log_Write_Nav_Tuning() {}
static void Log_Write_Performance() {}
static void Log_Write_Cmd(const AP_Mission::Mission_Command &cmd) {}
static int8_t process_logs(uint8_t argc, const Menu::arg *argv) { return 0; }
static void Log_Write_Control_Tuning() {}
static void Log_Write_Sonar() {}
......
APMrover2/Parameters.h
View file @ 62e7778b
......@@ -28,19 +28,33 @@ public:
// Misc
//
k_param_log_bitmask = 10,
k_param_log_bitmask_old = 10, // unused
k_param_num_resets,
k_param_reset_switch_chan,
k_param_initial_mode,
k_param_scheduler,
k_param_relay,
k_param_BoardConfig,
k_param_pivot_turn_angle,
k_param_rc_13,
k_param_rc_14,
// IO pins
k_param_rssi_pin = 20,
k_param_battery_volt_pin,
k_param_battery_curr_pin,
// braking
k_param_braking_percent = 30,
k_param_braking_speederr,
// misc2
k_param_log_bitmask = 40,
k_param_gps,
k_param_serial0_baud,
k_param_serial1_baud,
k_param_serial2_baud,
// 110: Telemetry control
//
......@@ -48,18 +62,21 @@ public:
k_param_gcs1, // stream rates for uartC
k_param_sysid_this_mav,
k_param_sysid_my_gcs,
k_param_serial0_baud,
k_param_serial1_baud,
k_param_serial0_baud_old,
k_param_serial1_baud_old,
k_param_telem_delay,
k_param_skip_gyro_cal,
k_param_gcs2, // stream rates for uartD
k_param_serial2_baud,
k_param_serial2_baud_old,
k_param_serial2_protocol,
//
// 130: Sensor parameters
//
k_param_compass_enabled = 130,
k_param_steering_learn, // unused
k_param_NavEKF, // Extended Kalman Filter Inertial Navigation Group
k_param_mission, // mission library
// 140: battery controls
k_param_battery_monitoring = 140, // deprecated, can be deleted
......@@ -113,12 +130,13 @@ public:
// obstacle control
k_param_sonar_enabled = 190, // deprecated, can be removed
k_param_sonar, // sonar object
k_param_sonar_old, // unused
k_param_sonar_trigger_cm,
k_param_sonar_turn_angle,
k_param_sonar_turn_time,
k_param_sonar2, // sonar2 object
k_param_sonar2_old, // unused
k_param_sonar_debounce,
k_param_sonar, // sonar object
//
// 210: driving modes
......@@ -135,8 +153,8 @@ public:
//
// 220: Waypoint data
//
k_param_command_total = 220,
k_param_command_index,
k_param_command_total = 220, // unused
k_param_command_index, // unused
k_param_waypoint_radius,
//
......@@ -166,6 +184,7 @@ public:
k_param_rcmap,
k_param_L1_controller,
k_param_steerController,
k_param_barometer,
// 254,255: reserved
};
......@@ -175,7 +194,7 @@ public:
// Misc
//
AP_Int16 log_bitmask;
AP_Int32 log_bitmask;
AP_Int16 num_resets;
AP_Int8 reset_switch_chan;
AP_Int8 initial_mode;
......@@ -183,14 +202,19 @@ public:
// IO pins
AP_Int8 rssi_pin;
// braking
AP_Int8 braking_percent;
AP_Float braking_speederr;
// Telemetry control
//
AP_Int16 sysid_this_mav;
AP_Int16 sysid_my_gcs;
AP_Int8 serial0_baud;
AP_Int8 serial1_baud;
AP_Int16 serial0_baud;
AP_Int16 serial1_baud;
#if MAVLINK_COMM_NUM_BUFFERS > 2
AP_Int8 serial2_baud;
AP_Int16 serial2_baud;
AP_Int8 serial2_protocol;
#endif
AP_Int8 telem_delay;
AP_Int8 skip_gyro_cal;
......@@ -207,6 +231,7 @@ public:
AP_Int8 auto_trigger_pin;
AP_Float auto_kickstart;
AP_Float turn_max_g;
AP_Int16 pivot_turn_angle;
// RC channels
RC_Channel rc_1;
......@@ -217,15 +242,17 @@ public:
RC_Channel_aux rc_6;
RC_Channel_aux rc_7;
RC_Channel_aux rc_8;
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
RC_Channel_aux rc_9;
#endif
#if CONFIG_HAL_BOARD == HAL_BOARD_APM2 || CONFIG_HAL_BOARD == HAL_BOARD_PX4
#if CONFIG_HAL_BOARD == HAL_BOARD_APM2 || CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
RC_Channel_aux rc_10;
RC_Channel_aux rc_11;
#endif
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
RC_Channel_aux rc_12;
RC_Channel_aux rc_13;
RC_Channel_aux rc_14;
#endif
// Throttle
......@@ -264,8 +291,6 @@ public:
// Waypoints
//
AP_Int8 command_total;
AP_Int8 command_index;
AP_Float waypoint_radius;
// PID controllers
......@@ -282,15 +307,17 @@ public:
rc_6(CH_6),
rc_7(CH_7),
rc_8(CH_8),
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
rc_9 (CH_9),
#endif
#if CONFIG_HAL_BOARD == HAL_BOARD_APM2 || CONFIG_HAL_BOARD == HAL_BOARD_PX4
#if CONFIG_HAL_BOARD == HAL_BOARD_APM2 || CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
rc_10 (CH_10),
rc_11 (CH_11),
#endif
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
rc_12 (CH_12),
rc_13 (CH_13),
rc_14 (CH_14),
#endif
// PID controller initial P initial I initial D initial imax
......
APMrover2/Parameters.pde
View file @ 62e7778b
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
/*
ArduPlane parameter definitions
APMRover2 parameter definitions
*/
#define GSCALAR(v, name, def) { g.v.vtype, name, Parameters::k_param_ ## v, &g.v, {def_value:def} }
......@@ -38,7 +38,7 @@ const AP_Param::Info var_info[] PROGMEM = {
// @Param: RSSI_PIN
// @DisplayName: Receiver RSSI sensing pin
// @Description: This selects an analog pin for the receiver RSSI voltage. It assumes the voltage is 5V for max rssi, 0V for minimum
// @Values: -1:Disabled, 0:A0, 1:A1, 13:A13
// @Values: -1:Disabled, 0:APM2 A0, 1:APM2 A1, 2:APM2 A2, 13:APM2 A13, 103:Pixhawk SBUS
// @User: Standard
GSCALAR(rssi_pin, "RSSI_PIN", -1),
......@@ -58,26 +58,37 @@ const AP_Param::Info var_info[] PROGMEM = {
// @Param: SERIAL0_BAUD
// @DisplayName: USB Console Baud Rate
// @Description: The baud rate used on the USB console
// @Values: 1:1200,2:2400,4:4800,9:9600,19:19200,38:38400,57:57600,111:111100,115:115200
// @Description: The baud rate used on the USB console. The APM2 can support all baudrates up to 115, and also can support 500. The PX4 can support rates of up to 1500. If you setup a rate you cannot support on APM2 and then can't connect to your board you should load a firmware from a different vehicle type. That will reset all your parameters to defaults.
// @Values: 1:1200,2:2400,4:4800,9:9600,19:19200,38:38400,57:57600,111:111100,115:115200,500:500000,921:921600,1500:1500000
// @User: Standard
GSCALAR(serial0_baud, "SERIAL0_BAUD", 115),
GSCALAR(serial0_baud, "SERIAL0_BAUD", SERIAL0_BAUD/1000),
// @Param: SERIAL1_BAUD
// @DisplayName: Telemetry Baud Rate
// @Description: The baud rate used on the first telemetry port
// @Values: 1:1200,2:2400,4:4800,9:9600,19:19200,38:38400,57:57600,111:111100,115:115200
// @Description: The baud rate used on the first telemetry port. The APM2 can support all baudrates up to 115, and also can support 500. The PX4 can support rates of up to 1500. If you setup a rate you cannot support on APM2 and then can't connect to your board you should load a firmware from a different vehicle type. That will reset all your parameters to defaults.
// @Values: 1:1200,2:2400,4:4800,9:9600,19:19200,38:38400,57:57600,111:111100,115:115200,500:500000,921:921600,1500:1500000
// @User: Standard
GSCALAR(serial1_baud, "SERIAL1_BAUD", 57),
GSCALAR(serial1_baud, "SERIAL1_BAUD", SERIAL1_BAUD/1000),
#if MAVLINK_COMM_NUM_BUFFERS > 2
// @Param: SERIAL2_BAUD
// @DisplayName: Telemetry Baud Rate
// @Description: The baud rate used on the second telemetry port (where available)
// @Values: 1:1200,2:2400,4:4800,9:9600,19:19200,38:38400,57:57600,111:111100,115:115200
// @Description: The baud rate used on the second telemetry port. The APM2 can support all baudrates up to 115, and also can support 500. The PX4 can support rates of up to 1500. If you setup a rate you cannot support on APM2 and then can't connect to your board you should load a firmware from a different vehicle type. That will reset all your parameters to defaults.
// @Values: 1:1200,2:2400,4:4800,9:9600,19:19200,38:38400,57:57600,111:111100,115:115200,500:500000,921:921600,1500:1500000
// @User: Standard
GSCALAR(serial2_baud, "SERIAL2_BAUD", 57),
#endif
GSCALAR(serial2_baud, "SERIAL2_BAUD", SERIAL2_BAUD/1000),
#if FRSKY_TELEM_ENABLED == ENABLED
// @Param: SERIAL2_PROTOCOL
// @DisplayName: SERIAL2 protocol selection
// @Description: Control what protocol telemetry 2 port should be used for
// @Values: 1:GCS Mavlink,2:Frsky D-PORT
// @User: Standard
GSCALAR(serial2_protocol, "SERIAL2_PROTOCOL", SERIAL2_MAVLINK),
#endif // FRSKY_TELEM_ENABLED
#endif // MAVLINK_COMM_NUM_BUFFERS
// @Param: TELEM_DELAY
// @DisplayName: Telemetry startup delay
......@@ -145,6 +156,33 @@ const AP_Param::Info var_info[] PROGMEM = {
// @User: Standard
GSCALAR(speed_turn_dist, "SPEED_TURN_DIST", 2.0f),
// @Param: BRAKING_PERCENT
// @DisplayName: Percentage braking to apply
// @Description: The maximum reverse throttle braking percentage to apply when cornering
// @Units: percent
// @Range: 0 100
// @Increment: 1
// @User: Standard
GSCALAR(braking_percent, "BRAKING_PERCENT", 0),
// @Param: BRAKING_SPEEDERR
// @DisplayName: Speed error at which to apply braking
// @Description: The amount of overspeed error at which to start applying braking
// @Units: m/s
// @Range: 0 100
// @Increment: 1
// @User: Standard
GSCALAR(braking_speederr, "BRAKING_SPEEDERR", 3),
// @Param: PIVOT_TURN_ANGLE
// @DisplayName: Pivot turn angle
// @Description: Navigation angle threshold in degrees to switch to pivot steering when SKID_STEER_OUT is 1. This allows you to setup a skid steering rover to turn on the spot in auto mode when the angle it needs to turn it greater than this angle. An angle of zero means to disable pivot turning. Note that you will probably also want to set a low value for WP_RADIUS to get neat turns.
// @Units: degrees
// @Range: 0 360
// @Increment: 1
// @User: Standard
GSCALAR(pivot_turn_angle, "PIVOT_TURN_ANGLE", 30),
// @Param: CH7_OPTION
// @DisplayName: Channel 7 option
// @Description: What to do use channel 7 for
......@@ -184,13 +222,13 @@ const AP_Param::Info var_info[] PROGMEM = {
// @Path: ../libraries/RC_Channel/RC_Channel.cpp,../libraries/RC_Channel/RC_Channel_aux.cpp
GGROUP(rc_8, "RC8_", RC_Channel_aux),
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
// @Group: RC9_
// @Path: ../libraries/RC_Channel/RC_Channel.cpp,../libraries/RC_Channel/RC_Channel_aux.cpp
GGROUP(rc_9, "RC9_", RC_Channel_aux),
#endif
#if CONFIG_HAL_BOARD == HAL_BOARD_APM2 || CONFIG_HAL_BOARD == HAL_BOARD_PX4
#if CONFIG_HAL_BOARD == HAL_BOARD_APM2 || CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
// @Group: RC10_
// @Path: ../libraries/RC_Channel/RC_Channel.cpp,../libraries/RC_Channel/RC_Channel_aux.cpp
GGROUP(rc_10, "RC10_", RC_Channel_aux),
......@@ -200,10 +238,18 @@ const AP_Param::Info var_info[] PROGMEM = {
GGROUP(rc_11, "RC11_", RC_Channel_aux),
#endif
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
// @Group: RC12_
// @Path: ../libraries/RC_Channel/RC_Channel.cpp,../libraries/RC_Channel/RC_Channel_aux.cpp
GGROUP(rc_12, "RC12_", RC_Channel_aux),
// @Group: RC13_
// @Path: ../libraries/RC_Channel/RC_Channel.cpp,../libraries/RC_Channel/RC_Channel_aux.cpp
GGROUP(rc_13, "RC13_", RC_Channel_aux),
// @Group: RC14_
// @Path: ../libraries/RC_Channel/RC_Channel.cpp,../libraries/RC_Channel/RC_Channel_aux.cpp
GGROUP(rc_14, "RC14_", RC_Channel_aux),
#endif
// @Param: THR_MIN
......@@ -235,12 +281,12 @@ const AP_Param::Info var_info[] PROGMEM = {
// @Param: THR_SLEWRATE
// @DisplayName: Throttle slew rate
// @Description: maximum percentage change in throttle per second. A setting of 10 means to not change the throttle by more than 10% of the full throttle range in one second. A value of zero means no limit.
// @Description: maximum percentage change in throttle per second. A setting of 10 means to not change the throttle by more than 10% of the full throttle range in one second. A value of zero means no limit. A value of 100 means the throttle can change over its full range in one second. Note that for some NiMH powered rovers setting a lower value like 40 or 50 may be worthwhile as the sudden current demand on the battery of a big rise in throttle may cause a brownout.
// @Units: Percent
// @Range: 0 100
// @Increment: 1
// @User: Standard
GSCALAR(throttle_slewrate, "THR_SLEWRATE", 0),
GSCALAR(throttle_slewrate, "THR_SLEWRATE", 100),
// @Param: SKID_STEER_OUT
// @DisplayName: Skid steering output
......@@ -292,40 +338,40 @@ const AP_Param::Info var_info[] PROGMEM = {
// @User: Standard
GSCALAR(fs_gcs_enabled, "FS_GCS_ENABLE", 0),
// @Param: SONAR_TRIGGER_CM
// @DisplayName: Sonar trigger distance
// @Description: The distance from an obstacle in centimeters at which the sonar triggers a turn to avoid the obstacle
// @Param: RNGFND_TRIGGR_CM
// @DisplayName: Rangefinder trigger distance
// @Description: The distance from an obstacle in centimeters at which the rangefinder triggers a turn to avoid the obstacle
// @Units: centimeters
// @Range: 0 1000
// @Increment: 1
// @User: Standard
GSCALAR(sonar_trigger_cm, "SONAR_TRIGGER_CM", 100),
GSCALAR(sonar_trigger_cm, "RNGFND_TRIGGR_CM", 100),
// @Param: SONAR_TURN_ANGLE
// @DisplayName: Sonar trigger angle
// @Description: The course deviation in degrees to apply while avoiding an obstacle detected with the sonar. A positive number means to turn right, and a negative angle means to turn left.
// @Param: RNGFND_TURN_ANGL
// @DisplayName: Rangefinder trigger angle
// @Description: The course deviation in degrees to apply while avoiding an obstacle detected with the rangefinder. A positive number means to turn right, and a negative angle means to turn left.
// @Units: centimeters
// @Range: -45 45
// @Increment: 1
// @User: Standard
GSCALAR(sonar_turn_angle, "SONAR_TURN_ANGLE", 45),
GSCALAR(sonar_turn_angle, "RNGFND_TURN_ANGL", 45),
// @Param: SONAR_TURN_TIME
// @DisplayName: Sonar turn time
// @Description: The amount of time in seconds to apply the SONAR_TURN_ANGLE after detecting an obstacle.
// @Param: RNGFND_TURN_TIME
// @DisplayName: Rangefinder turn time
// @Description: The amount of time in seconds to apply the RNGFND_TURN_ANGL after detecting an obstacle.
// @Units: seconds
// @Range: 0 100
// @Increment: 0.1
// @User: Standard
GSCALAR(sonar_turn_time, "SONAR_TURN_TIME", 1.0f),
GSCALAR(sonar_turn_time, "RNGFND_TURN_TIME", 1.0f),
// @Param: SONAR_DEBOUNCE
// @DisplayName: Sonar debounce count
// @Description: The number of 50Hz sonar hits needed to trigger an obstacle avoidance event. If you get a lot of false sonar events then raise this number, but if you make it too large then it will cause lag in detecting obstacles, which could cause you go hit the obstacle.
// @Param: RNGFND_DEBOUNCE
// @DisplayName: Rangefinder debounce count
// @Description: The number of 50Hz rangefinder hits needed to trigger an obstacle avoidance event. If you get a lot of false sonar events then raise this number, but if you make it too large then it will cause lag in detecting obstacles, which could cause you go hit the obstacle.
// @Range: 1 100
// @Increment: 1
// @User: Standard
GSCALAR(sonar_debounce, "SONAR_DEBOUNCE", 2),
GSCALAR(sonar_debounce, "RNGFND_DEBOUNCE", 2),
// @Param: LEARN_CH
// @DisplayName: Learning channel
......@@ -381,9 +427,6 @@ const AP_Param::Info var_info[] PROGMEM = {
// @User: Standard
GSCALAR(mode6, "MODE6", MODE_6),
GSCALAR(command_total, "CMD_TOTAL", 0),
GSCALAR(command_index, "CMD_INDEX", 0),
// @Param: WP_RADIUS
// @DisplayName: Waypoint radius
// @Description: The distance in meters from a waypoint when we consider the waypoint has been reached. This determines when the rover will turn along the next waypoint path.
......@@ -418,6 +461,12 @@ const AP_Param::Info var_info[] PROGMEM = {
// @Path: ../libraries/AP_Scheduler/AP_Scheduler.cpp
GOBJECT(scheduler, "SCHED_", AP_Scheduler),
// 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: RELAY_
// @Path: ../libraries/AP_Relay/AP_Relay.cpp
GOBJECT(relay, "RELAY_", AP_Relay),
......@@ -444,13 +493,9 @@ const AP_Param::Info var_info[] PROGMEM = {
// @Path: ../libraries/AP_L1_Control/AP_L1_Control.cpp
GOBJECT(L1_controller, "NAVL1_", AP_L1_Control),
// @Group: SONAR_
// @Path: ../libraries/AP_RangeFinder/AP_RangeFinder_analog.cpp
GOBJECT(sonar, "SONAR_", AP_RangeFinder_analog),
// @Group: SONAR2_
// @Path: ../libraries/AP_RangeFinder/AP_RangeFinder_analog.cpp
GOBJECT(sonar2, "SONAR2_", AP_RangeFinder_analog),
// @Group: RNGFND
// @Path: ../libraries/AP_RangeFinder/RangeFinder.cpp
GOBJECT(sonar, "RNGFND", RangeFinder),
// @Group: INS_
// @Path: ../libraries/AP_InertialSensor/AP_InertialSensor.cpp
......@@ -480,12 +525,27 @@ 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
GOBJECT(BoardConfig, "BRD_", AP_BoardConfig),
// GPS driver
// @Group: GPS_
// @Path: ../libraries/AP_GPS/AP_GPS.cpp
GOBJECT(gps, "GPS_", AP_GPS),
#if AP_AHRS_NAVEKF_AVAILABLE
// @Group: EKF_
// @Path: ../libraries/AP_NavEKF/AP_NavEKF.cpp
GOBJECTN(ahrs.get_NavEKF(), NavEKF, "EKF_", NavEKF),
#endif
// @Group: MIS_
// @Path: ../libraries/AP_Mission/AP_Mission.cpp
GOBJECT(mission, "MIS_", AP_Mission),
AP_VAREND
};
......@@ -513,6 +573,11 @@ const AP_Param::ConversionInfo conversion_table[] PROGMEM = {
static void load_parameters(void)
{
if (!AP_Param::check_var_info()) {
cliSerial->printf_P(PSTR("Bad var table\n"));
hal.scheduler->panic(PSTR("Bad var table"));
}
if (!g.format_version.load() ||
g.format_version != Parameters::k_format_version) {
......
APMrover2/Steering.pde
View file @ 62e7778b
......@@ -6,7 +6,7 @@
static void throttle_slew_limit(int16_t last_throttle)
{
// if slew limit rate is set to zero then do not slew limit
if (g.throttle_slewrate) {
if (g.throttle_slewrate && last_throttle != 0) {
// limit throttle change by the given percentage per second
float temp = g.throttle_slewrate * G_Dt * 0.01f * fabsf(channel_throttle->radio_max - channel_throttle->radio_min);
// allow a minimum change of 1 PWM per cycle
......@@ -64,6 +64,20 @@ static bool auto_check_trigger(void)
return false;
}
/*
work out if we are going to use pivot steering
*/
static bool use_pivot_steering(void)
{
if (control_mode >= AUTO && g.skid_steer_out && g.pivot_turn_angle != 0) {
int16_t bearing_error = wrap_180_cd(nav_controller->target_bearing_cd() - ahrs.yaw_sensor) / 100;
if (abs(bearing_error) > g.pivot_turn_angle) {
return true;
}
}
return false;
}
/*
calculate the throtte for auto-throttle modes
......@@ -84,11 +98,18 @@ static void calc_throttle(float target_speed)
*/
float steer_rate = fabsf(lateral_acceleration / (g.turn_max_g*GRAVITY_MSS));
steer_rate = constrain_float(steer_rate, 0.0, 1.0);
float reduction = 1.0 - steer_rate*(100 - g.speed_turn_gain)*0.01;
// use g.speed_turn_gain for a 90 degree turn, and in proportion
// for other turn angles
int32_t turn_angle = wrap_180_cd(next_navigation_leg_cd - ahrs.yaw_sensor);
float speed_turn_ratio = constrain_float(fabsf(turn_angle / 9000.0f), 0, 1);
float speed_turn_reduction = (100 - g.speed_turn_gain) * speed_turn_ratio * 0.01f;
float reduction = 1.0 - steer_rate*speed_turn_reduction;
if (control_mode >= AUTO && wp_distance <= g.speed_turn_dist) {
// in auto-modes we reduce speed when approaching waypoints
float reduction2 = 1.0 - (100-g.speed_turn_gain)*0.01*((g.speed_turn_dist - wp_distance)/g.speed_turn_dist);
float reduction2 = 1.0 - speed_turn_reduction;
if (reduction2 < reduction) {
reduction = reduction2;
}
......@@ -110,6 +131,27 @@ static void calc_throttle(float target_speed)
} else {
channel_throttle->servo_out = constrain_int16(throttle, g.throttle_min, g.throttle_max);
}
if (!in_reverse && g.braking_percent != 0 && groundspeed_error < -g.braking_speederr) {
// the user has asked to use reverse throttle to brake. Apply
// it in proportion to the ground speed error, but only when
// our ground speed error is more than BRAKING_SPEEDERR.
//
// We use a linear gain, with 0 gain at a ground speed error
// of braking_speederr, and 100% gain when groundspeed_error
// is 2*braking_speederr
float brake_gain = constrain_float(((-groundspeed_error)-g.braking_speederr)/g.braking_speederr, 0, 1);
int16_t braking_throttle = g.throttle_max * (g.braking_percent * 0.01f) * brake_gain;
channel_throttle->servo_out = constrain_int16(-braking_throttle, -g.throttle_max, -g.throttle_min);
// temporarily set us in reverse to allow the PWM setting to
// go negative
set_reverse(true);
}
if (use_pivot_steering()) {
channel_throttle->servo_out = 0;
}
}
/*****************************************
......@@ -137,6 +179,14 @@ static void calc_lateral_acceleration()
// negative error = left turn
// positive error = right turn
lateral_acceleration = nav_controller->lateral_acceleration();
if (use_pivot_steering()) {
int16_t bearing_error = wrap_180_cd(nav_controller->target_bearing_cd() - ahrs.yaw_sensor) / 100;
if (bearing_error > 0) {
lateral_acceleration = g.turn_max_g*GRAVITY_MSS;
} else {
lateral_acceleration = -g.turn_max_g*GRAVITY_MSS;
}
}
}
/*
......@@ -144,13 +194,6 @@ static void calc_lateral_acceleration()
*/
static void calc_nav_steer()
{
float speed = g_gps->ground_speed_cm * 0.01f;
if (speed < 1.0f) {
// gps speed isn't very accurate at low speed
speed = 1.0f;
}
// add in obstacle avoidance
lateral_acceleration += (obstacle.turn_angle/45.0f) * g.turn_max_g;
......@@ -165,10 +208,12 @@ static void calc_nav_steer()
*****************************************/
static void set_servos(void)
{
int16_t last_throttle = channel_throttle->radio_out;
static int16_t last_throttle;
// support a separate steering channel
RC_Channel_aux::set_servo_out(RC_Channel_aux::k_steering, channel_steer->pwm_to_angle_dz(0));
if ((control_mode == MANUAL || control_mode == LEARNING) &&
(g.skid_steer_out == g.skid_steer_in)) {
if (control_mode == MANUAL || control_mode == LEARNING) {
// do a direct pass through of radio values
channel_steer->radio_out = channel_steer->read();
channel_throttle->radio_out = channel_throttle->read();
......@@ -198,25 +243,28 @@ static void set_servos(void)
// limit throttle movement speed
throttle_slew_limit(last_throttle);
}
if (g.skid_steer_out) {
// convert the two radio_out values to skid steering values
/*
mixing rule:
steering = motor1 - motor2
throttle = 0.5*(motor1 + motor2)
motor1 = throttle + 0.5*steering
motor2 = throttle - 0.5*steering
*/
float steering_scaled = channel_steer->norm_output();
float throttle_scaled = channel_throttle->norm_output();
float motor1 = throttle_scaled + 0.5*steering_scaled;
float motor2 = throttle_scaled - 0.5*steering_scaled;
channel_steer->servo_out = 4500*motor1;
channel_throttle->servo_out = 100*motor2;
channel_steer->calc_pwm();
channel_throttle->calc_pwm();
}
// record last throttle before we apply skid steering
last_throttle = channel_throttle->radio_out;
if (g.skid_steer_out) {
// convert the two radio_out values to skid steering values
/*
mixing rule:
steering = motor1 - motor2
throttle = 0.5*(motor1 + motor2)
motor1 = throttle + 0.5*steering
motor2 = throttle - 0.5*steering
*/
float steering_scaled = channel_steer->norm_output();
float throttle_scaled = channel_throttle->norm_output();
float motor1 = throttle_scaled + 0.5*steering_scaled;
float motor2 = throttle_scaled - 0.5*steering_scaled;
channel_steer->servo_out = 4500*motor1;
channel_throttle->servo_out = 100*motor2;
channel_steer->calc_pwm();
channel_throttle->calc_pwm();
}
......@@ -225,25 +273,7 @@ static void set_servos(void)
// ----------------------------------------
channel_steer->output();
channel_throttle->output();
// Route configurable aux. functions to their respective servos
g.rc_2.output_ch(CH_2);
g.rc_4.output_ch(CH_4);
g.rc_5.output_ch(CH_5);
g.rc_6.output_ch(CH_6);
g.rc_7.output_ch(CH_7);
g.rc_8.output_ch(CH_8);
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
g.rc_9.output_ch(CH_9);
#endif
#if CONFIG_HAL_BOARD == HAL_BOARD_APM2 || CONFIG_HAL_BOARD == HAL_BOARD_PX4
g.rc_10.output_ch(CH_10);
g.rc_11.output_ch(CH_11);
#endif
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
g.rc_12.output_ch(CH_12);
#endif
RC_Channel_aux::output_ch_all();
#endif
}
......
APMrover2/commands.pde
View file @ 62e7778b
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
/* Functions in this file:
void init_commands()
struct Location get_cmd_with_index(int i)
void set_cmd_with_index(struct Location temp, int i)
void increment_cmd_index()
void decrement_cmd_index()
long read_alt_to_hold()
void set_next_WP(struct Location *wp)
void set_next_WP(const AP_Mission::Mission_Command& cmd)
void set_guided_WP(void)
void init_home()
void restart_nav()
************************************************************
*/
static void init_commands()
{
g.command_index.set_and_save(0);
nav_command_ID = NO_COMMAND;
non_nav_command_ID = NO_COMMAND;
next_nav_command.id = CMD_BLANK;
}
// Getters
// -------
static struct Location get_cmd_with_index(int i)
{
struct Location temp;
uint16_t mem;
// Find out proper location in memory by using the start_byte position + the index
// --------------------------------------------------------------------------------
if (i > g.command_total) {
memset(&temp, 0, sizeof(temp));
temp.id = CMD_BLANK;
}else{
// read WP position
mem = (WP_START_BYTE) + (i * WP_SIZE);
temp.id = hal.storage->read_byte(mem);
mem++;
temp.options = hal.storage->read_byte(mem);
mem++;
temp.p1 = hal.storage->read_byte(mem);
mem++;
temp.alt = (long)hal.storage->read_dword(mem);
mem += 4;
temp.lat = (long)hal.storage->read_dword(mem);
mem += 4;
temp.lng = (long)hal.storage->read_dword(mem);
}
// Add on home altitude if we are a nav command (or other command with altitude) and stored alt is relative
if((temp.id < MAV_CMD_NAV_LAST || temp.id == MAV_CMD_CONDITION_CHANGE_ALT) && temp.options & MASK_OPTIONS_RELATIVE_ALT){
temp.alt += home.alt;
}
return temp;
}
// Setters
// -------
static void set_cmd_with_index(struct Location temp, int i)
{
i = constrain_int16(i, 0, g.command_total.get());
uint16_t mem = WP_START_BYTE + (i * WP_SIZE);
// Set altitude options bitmask
// XXX What is this trying to do?
if ((temp.options & MASK_OPTIONS_RELATIVE_ALT) && i != 0){
temp.options = MASK_OPTIONS_RELATIVE_ALT;
} else {
temp.options = 0;
}
hal.storage->write_byte(mem, temp.id);
mem++;
hal.storage->write_byte(mem, temp.options);
mem++;
hal.storage->write_byte(mem, temp.p1);
mem++;
hal.storage->write_dword(mem, temp.alt);
mem += 4;
hal.storage->write_dword(mem, temp.lat);
mem += 4;
hal.storage->write_dword(mem, temp.lng);
}
/*
This function stores waypoint commands
It looks to see what the next command type is and finds the last command.
*/
static void set_next_WP(const struct Location *wp)
* set_next_WP - sets the target location the vehicle should fly to
*/
static void set_next_WP(const struct Location& loc)
{
// copy the current WP into the OldWP slot
// ---------------------------------------
......@@ -108,7 +20,7 @@ static void set_next_WP(const struct Location *wp)
// Load the next_WP slot
// ---------------------
next_WP = *wp;
next_WP = loc;
// are we already past the waypoint? This happens when we jump
// waypoints, and it can cause us to skip a waypoint. If we are
......@@ -151,17 +63,11 @@ void init_home()
gcs_send_text_P(SEVERITY_LOW, PSTR("init home"));
home.id = MAV_CMD_NAV_WAYPOINT;
home.lng = g_gps->longitude; // Lon * 10**7
home.lat = g_gps->latitude; // Lat * 10**7
gps_base_alt = max(g_gps->altitude_cm, 0);
home.alt = g_gps->altitude_cm;
ahrs.set_home(gps.location());
home_is_set = true;
// Save Home to EEPROM - Command 0
// -------------------
set_cmd_with_index(home, 0);
// Save Home to EEPROM
mission.write_home_to_storage();
// Save prev loc
// -------------
......@@ -176,7 +82,5 @@ static void restart_nav()
{
g.pidSpeedThrottle.reset_I();
prev_WP = current_loc;
nav_command_ID = NO_COMMAND;
nav_command_index = 0;
process_next_command();
mission.start_or_resume();
}
APMrover2/commands_logic.pde
View file @ 62e7778b
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
// forward declarations to make compiler happy
static void do_nav_wp(const AP_Mission::Mission_Command& cmd);
static void do_wait_delay(const AP_Mission::Mission_Command& cmd);
static void do_within_distance(const AP_Mission::Mission_Command& cmd);
static void do_change_speed(const AP_Mission::Mission_Command& cmd);
static void do_set_home(const AP_Mission::Mission_Command& cmd);
static bool verify_nav_wp(const AP_Mission::Mission_Command& cmd);
/********************************************************************************/
// Command Event Handlers
/********************************************************************************/
static void
handle_process_nav_cmd()
static bool
start_command(const AP_Mission::Mission_Command& cmd)
{
gcs_send_text_fmt(PSTR("Executing command ID #%i"),next_nav_command.id);
// log when new commands start
if (should_log(MASK_LOG_CMD)) {
Log_Write_Cmd(cmd);
}
switch(next_nav_command.id){
case MAV_CMD_NAV_TAKEOFF:
do_takeoff();
break;
// exit immediately if not in AUTO mode
if (control_mode != AUTO) {
return false;
}
gcs_send_text_fmt(PSTR("Executing command ID #%i"),cmd.id);
// remember the course of our next navigation leg
next_navigation_leg_cd = mission.get_next_ground_course_cd(0);
switch(cmd.id){
case MAV_CMD_NAV_WAYPOINT: // Navigate to Waypoint
do_nav_wp();
do_nav_wp(cmd);
break;
case MAV_CMD_NAV_RETURN_TO_LAUNCH:
do_RTL();
break;
default:
break;
}
}
static void
handle_process_condition_command()
{
gcs_send_text_fmt(PSTR("Executing command ID #%i"),next_nonnav_command.id);
switch(next_nonnav_command.id){
// Conditional commands
case MAV_CMD_CONDITION_DELAY:
do_wait_delay();
do_wait_delay(cmd);
break;
case MAV_CMD_CONDITION_DISTANCE:
do_within_distance();
break;
case MAV_CMD_CONDITION_CHANGE_ALT:
do_change_alt();
break;
default:
break;
}
}
static void handle_process_do_command()
{
gcs_send_text_fmt(PSTR("Executing command ID #%i"),next_nonnav_command.id);
switch(next_nonnav_command.id){
case MAV_CMD_DO_JUMP:
do_jump();
do_within_distance(cmd);
break;
// Do commands
case MAV_CMD_DO_CHANGE_SPEED:
do_change_speed();
do_change_speed(cmd);
break;
case MAV_CMD_DO_SET_HOME:
do_set_home();
do_set_home(cmd);
break;
case MAV_CMD_DO_SET_SERVO:
ServoRelayEvents.do_set_servo(next_nonnav_command.p1, next_nonnav_command.alt);
ServoRelayEvents.do_set_servo(cmd.content.servo.channel, cmd.content.servo.pwm);
break;
case MAV_CMD_DO_SET_RELAY:
ServoRelayEvents.do_set_relay(next_nonnav_command.p1, next_nonnav_command.alt);
ServoRelayEvents.do_set_relay(cmd.content.relay.num, cmd.content.relay.state);
break;
case MAV_CMD_DO_REPEAT_SERVO:
ServoRelayEvents.do_repeat_servo(next_nonnav_command.p1, next_nonnav_command.alt,
next_nonnav_command.lat, next_nonnav_command.lng);
ServoRelayEvents.do_repeat_servo(cmd.content.repeat_servo.channel, cmd.content.repeat_servo.pwm,
cmd.content.repeat_servo.repeat_count, cmd.content.repeat_servo.cycle_time * 1000.0f);
break;
case MAV_CMD_DO_REPEAT_RELAY:
ServoRelayEvents.do_repeat_relay(next_nonnav_command.p1, next_nonnav_command.alt,
next_nonnav_command.lat);
ServoRelayEvents.do_repeat_relay(cmd.content.repeat_relay.num, cmd.content.repeat_relay.repeat_count,
cmd.content.repeat_relay.cycle_time * 1000.0f);
break;
#if CAMERA == ENABLED
case MAV_CMD_DO_CONTROL_VIDEO: // Control on-board camera capturing. |Camera ID (-1 for all)| Transmission: 0: disabled, 1: enabled compressed, 2: enabled raw| Transmission mode: 0: video stream, >0: single images every n seconds (decimal)| Recording: 0: disabled, 1: enabled compressed, 2: enabled raw| Empty| Empty| Empty|
break;
case MAV_CMD_DO_CONTROL_VIDEO: // Control on-board camera capturing. |Camera ID (-1 for all)| Transmission: 0: disabled, 1: enabled compressed, 2: enabled raw| Transmission mode: 0: video stream, >0: single images every n seconds (decimal)| Recording: 0: disabled, 1: enabled compressed, 2: enabled raw| Empty| Empty| Empty|
break;
case MAV_CMD_DO_DIGICAM_CONFIGURE: // Mission command to configure an on-board camera controller system. |Modes: P, TV, AV, M, Etc| Shutter speed: Divisor number for one second| Aperture: F stop number| ISO number e.g. 80, 100, 200, Etc| Exposure type enumerator| Command Identity| Main engine cut-off time before camera trigger in seconds/10 (0 means no cut-off)|
break;
case MAV_CMD_DO_DIGICAM_CONFIGURE: // Mission command to configure an on-board camera controller system. |Modes: P, TV, AV, M, Etc| Shutter speed: Divisor number for one second| Aperture: F stop number| ISO number e.g. 80, 100, 200, Etc| Exposure type enumerator| Command Identity| Main engine cut-off time before camera trigger in seconds/10 (0 means no cut-off)|
break;
case MAV_CMD_DO_DIGICAM_CONTROL: // Mission command to control an on-board camera controller system. |Session control e.g. show/hide lens| Zoom's absolute position| Zooming step value to offset zoom from the current position| Focus Locking, Unlocking or Re-locking| Shooting Command| Command Identity| Empty|
do_take_picture();
break;
case MAV_CMD_DO_DIGICAM_CONTROL: // Mission command to control an on-board camera controller system. |Session control e.g. show/hide lens| Zoom's absolute position| Zooming step value to offset zoom from the current position| Focus Locking, Unlocking or Re-locking| Shooting Command| Command Identity| Empty|
do_take_picture();
break;
case MAV_CMD_DO_SET_CAM_TRIGG_DIST:
camera.set_trigger_distance(next_nonnav_command.alt);
break;
case MAV_CMD_DO_SET_CAM_TRIGG_DIST:
camera.set_trigger_distance(cmd.content.cam_trigg_dist.meters);
break;
#endif
#if MOUNT == ENABLED
......@@ -106,12 +96,17 @@ static void handle_process_do_command()
// system to control the vehicle attitude and the attitude of various
// devices such as cameras.
// |Region of interest mode. (see MAV_ROI enum)| Waypoint index/ target ID. (see MAV_ROI enum)| ROI index (allows a vehicle to manage multiple cameras etc.)| Empty| x the location of the fixed ROI (see MAV_FRAME)| y| z|
case MAV_CMD_DO_SET_ROI:
#if 0
// not supported yet
camera_mount.set_roi_cmd();
#endif
break;
case MAV_CMD_DO_SET_ROI:
if (cmd.content.location.alt == 0 && cmd.content.location.lat == 0 && cmd.content.location.lng == 0) {
// switch off the camera tracking if enabled
if (camera_mount.get_mode() == MAV_MOUNT_MODE_GPS_POINT) {
camera_mount.set_mode_to_default();
}
} else {
// send the command to the camera mount
camera_mount.set_roi_cmd(&cmd.content.location);
}
break;
case MAV_CMD_DO_MOUNT_CONFIGURE: // Mission command to configure a camera mount |Mount operation mode (see MAV_CONFIGURE_MOUNT_MODE enum)| stabilize roll? (1 = yes, 0 = no)| stabilize pitch? (1 = yes, 0 = no)| stabilize yaw? (1 = yes, 0 = no)| Empty| Empty| Empty|
camera_mount.configure_cmd();
......@@ -121,64 +116,63 @@ static void handle_process_do_command()
camera_mount.control_cmd();
break;
#endif
default:
// return false for unhandled commands
return false;
}
// if we got this far we must have been successful
return true;
}
static void handle_no_commands()
{
gcs_send_text_fmt(PSTR("No commands - setting HOLD"));
set_mode(HOLD);
// exit_mission - callback function called from ap-mission when the mission has completed
// we double check that the flight mode is AUTO to avoid the possibility of ap-mission triggering actions while we're not in AUTO mode
static void exit_mission()
{
if (control_mode == AUTO) {
gcs_send_text_fmt(PSTR("No commands - setting HOLD"));
set_mode(HOLD);
}
}
/********************************************************************************/
// Verify command Handlers
// Returns true if command complete
/********************************************************************************/
static bool verify_nav_command() // Returns true if command complete
static bool verify_command(const AP_Mission::Mission_Command& cmd)
{
switch(nav_command_ID) {
// exit immediately if not in AUTO mode
// we return true or we will continue to be called by ap-mission
if (control_mode != AUTO) {
return true;
}
case MAV_CMD_NAV_TAKEOFF:
return verify_takeoff();
switch(cmd.id) {
case MAV_CMD_NAV_WAYPOINT:
return verify_nav_wp();
return verify_nav_wp(cmd);
case MAV_CMD_NAV_RETURN_TO_LAUNCH:
return verify_RTL();
default:
gcs_send_text_P(SEVERITY_HIGH,PSTR("verify_nav: Invalid or no current Nav cmd"));
return false;
}
}
case MAV_CMD_CONDITION_DELAY:
return verify_wait_delay();
break;
static bool verify_condition_command() // Returns true if command complete
{
switch(non_nav_command_ID) {
case NO_COMMAND:
break;
case MAV_CMD_CONDITION_DELAY:
return verify_wait_delay();
break;
case MAV_CMD_CONDITION_DISTANCE:
return verify_within_distance();
break;
case MAV_CMD_CONDITION_CHANGE_ALT:
return verify_change_alt();
break;
case WAIT_COMMAND:
return 0;
break;
default:
gcs_send_text_P(SEVERITY_HIGH,PSTR("verify_conditon: Invalid or no current Condition cmd"));
break;
case MAV_CMD_CONDITION_DISTANCE:
return verify_within_distance();
break;
default:
if (cmd.id > MAV_CMD_CONDITION_LAST) {
// this is a command that doesn't require verify
return true;
}
gcs_send_text_P(SEVERITY_HIGH,PSTR("verify_conditon: Unsupported command"));
return true;
break;
}
return false;
}
......@@ -189,33 +183,24 @@ static bool verify_condition_command() // Returns true if command complete
static void do_RTL(void)
{
prev_WP = current_loc;
prev_WP = current_loc;
control_mode = RTL;
next_WP = home;
}
static void do_takeoff()
{
set_next_WP(&next_nav_command);
next_WP = home;
}
static void do_nav_wp()
static void do_nav_wp(const AP_Mission::Mission_Command& cmd)
{
set_next_WP(&next_nav_command);
set_next_WP(cmd.content.location);
}
/********************************************************************************/
// Verify Nav (Must) commands
/********************************************************************************/
static bool verify_takeoff()
{ return true;
}
static bool verify_nav_wp()
static bool verify_nav_wp(const AP_Mission::Mission_Command& cmd)
{
if ((wp_distance > 0) && (wp_distance <= g.waypoint_radius)) {
gcs_send_text_fmt(PSTR("Reached Waypoint #%i dist %um"),
(unsigned)nav_command_index,
(unsigned)cmd.index,
(unsigned)get_distance(current_loc, next_WP));
return true;
}
......@@ -223,7 +208,7 @@ static bool verify_nav_wp()
// have we gone past the waypoint?
if (location_passed_point(current_loc, prev_WP, next_WP)) {
gcs_send_text_fmt(PSTR("Passed Waypoint #%i dist %um"),
(unsigned)nav_command_index,
(unsigned)cmd.index,
(unsigned)get_distance(current_loc, next_WP));
return true;
}
......@@ -253,23 +238,15 @@ static bool verify_RTL()
// Condition (May) commands
/********************************************************************************/
static void do_wait_delay()
static void do_wait_delay(const AP_Mission::Mission_Command& cmd)
{
condition_start = millis();
condition_value = next_nonnav_command.lat * 1000; // convert to milliseconds
}
static void do_change_alt()
{
condition_rate = abs((int)next_nonnav_command.lat);
condition_value = next_nonnav_command.alt;
if(condition_value < current_loc.alt) condition_rate = -condition_rate;
next_WP.alt = condition_value; // For future nav calculations
condition_value = cmd.content.delay.seconds * 1000; // convert seconds to milliseconds
}
static void do_within_distance()
static void do_within_distance(const AP_Mission::Mission_Command& cmd)
{
condition_value = next_nonnav_command.lat;
condition_value = cmd.content.distance.meters;
}
/********************************************************************************/
......@@ -285,15 +262,6 @@ static bool verify_wait_delay()
return false;
}
static bool verify_change_alt()
{
if( (condition_rate>=0 && current_loc.alt >= condition_value) || (condition_rate<=0 && current_loc.alt <= condition_value)) {
condition_value = 0;
return true;
}
return false;
}
static bool verify_within_distance()
{
if (wp_distance < condition_value){
......@@ -307,63 +275,30 @@ static bool verify_within_distance()
// Do (Now) commands
/********************************************************************************/
static void do_jump()
{
struct Location temp;
gcs_send_text_fmt(PSTR("In jump. Jumps left: %i"),next_nonnav_command.lat);
if(next_nonnav_command.lat > 0) {
nav_command_ID = NO_COMMAND;
next_nav_command.id = NO_COMMAND;
non_nav_command_ID = NO_COMMAND;
temp = get_cmd_with_index(g.command_index);
temp.lat = next_nonnav_command.lat - 1; // Decrement repeat counter
set_cmd_with_index(temp, g.command_index);
gcs_send_text_fmt(PSTR("setting command index: %i"),next_nonnav_command.p1 - 1);
g.command_index.set_and_save(next_nonnav_command.p1 - 1);
nav_command_index = next_nonnav_command.p1 - 1;
next_WP = prev_WP; // Need to back "next_WP" up as it was set to the next waypoint following the jump
process_next_command();
} else if (next_nonnav_command.lat == -1) { // A repeat count of -1 = repeat forever
nav_command_ID = NO_COMMAND;
non_nav_command_ID = NO_COMMAND;
gcs_send_text_fmt(PSTR("setting command index: %i"),next_nonnav_command.p1 - 1);
g.command_index.set_and_save(next_nonnav_command.p1 - 1);
nav_command_index = next_nonnav_command.p1 - 1;
next_WP = prev_WP; // Need to back "next_WP" up as it was set to the next waypoint following the jump
process_next_command();
}
}
static void do_change_speed()
static void do_change_speed(const AP_Mission::Mission_Command& cmd)
{
switch (next_nonnav_command.p1)
switch (cmd.p1)
{
case 0:
if (next_nonnav_command.alt > 0) {
g.speed_cruise.set(next_nonnav_command.alt);
gcs_send_text_fmt(PSTR("Cruise speed: %.1f"), g.speed_cruise.get());
if (cmd.content.speed.target_ms > 0) {
g.speed_cruise.set(cmd.content.speed.target_ms);
gcs_send_text_fmt(PSTR("Cruise speed: %.1f m/s"), g.speed_cruise.get());
}
break;
}
if (next_nonnav_command.lat > 0) {
g.throttle_cruise.set(next_nonnav_command.lat);
if (cmd.content.speed.throttle_pct > 0 && cmd.content.speed.throttle_pct <= 100) {
g.throttle_cruise.set(cmd.content.speed.throttle_pct);
gcs_send_text_fmt(PSTR("Cruise throttle: %.1f"), g.throttle_cruise.get());
}
}
static void do_set_home()
static void do_set_home(const AP_Mission::Mission_Command& cmd)
{
if(next_nonnav_command.p1 == 1 && have_position) {
if(cmd.p1 == 1 && have_position) {
init_home();
} else {
home.id = MAV_CMD_NAV_WAYPOINT;
home.lng = next_nonnav_command.lng; // Lon * 10**7
home.lat = next_nonnav_command.lat; // Lat * 10**7
home.alt = max(next_nonnav_command.alt, 0);
ahrs.set_home(cmd.content.location);
home_is_set = true;
}
}
......@@ -373,8 +308,9 @@ static void do_take_picture()
{
#if CAMERA == ENABLED
camera.trigger_pic();
gcs_send_message(MSG_CAMERA_FEEDBACK);
if (should_log(MASK_LOG_CAMERA)) {
Log_Write_Camera();
DataFlash.Log_Write_Camera(ahrs, gps, current_loc);
}
#endif
}
APMrover2/commands_process.pde
View file @ 62e7778b
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
// For changing active command mid-mission
//----------------------------------------
static void change_command(uint8_t cmd_index)
{
struct Location temp = get_cmd_with_index(cmd_index);
if (temp.id > MAV_CMD_NAV_LAST ){
gcs_send_text_P(SEVERITY_LOW,PSTR("Bad Request - cannot change to non-Nav cmd"));
} else {
gcs_send_text_fmt(PSTR("Received Request - jump to command #%i"),cmd_index);
nav_command_ID = NO_COMMAND;
next_nav_command.id = NO_COMMAND;
non_nav_command_ID = NO_COMMAND;
nav_command_index = cmd_index - 1;
g.command_index.set_and_save(cmd_index);
update_commands();
}
}
// called by 10 Hz loop
// called by update navigation at 10Hz
// --------------------
static void update_commands(void)
{
if(control_mode == AUTO){
if(home_is_set == true && g.command_total > 1){
process_next_command();
}
} // Other (eg GCS_Auto) modes may be implemented here
}
static void verify_commands(void)
{
if(verify_nav_command()){
nav_command_ID = NO_COMMAND;
}
if(verify_condition_command()){
non_nav_command_ID = NO_COMMAND;
}
}
static void process_next_command()
{
// This function makes sure that we always have a current navigation command
// and loads conditional or immediate commands if applicable
struct Location temp;
uint8_t old_index = 0;
// these are Navigation/Must commands
// ---------------------------------
if (nav_command_ID == NO_COMMAND){ // no current navigation command loaded
old_index = nav_command_index;
temp.id = MAV_CMD_NAV_LAST;
while(temp.id >= MAV_CMD_NAV_LAST && nav_command_index <= g.command_total) {
nav_command_index++;
temp = get_cmd_with_index(nav_command_index);
}
gcs_send_text_fmt(PSTR("Nav command index updated to #%i"),nav_command_index);
if(nav_command_index > g.command_total){
handle_no_commands();
} else {
next_nav_command = temp;
nav_command_ID = next_nav_command.id;
non_nav_command_index = NO_COMMAND; // This will cause the next intervening non-nav command (if any) to be loaded
non_nav_command_ID = NO_COMMAND;
process_nav_cmd();
}
}
// these are Condition/May and Do/Now commands
// -------------------------------------------
if (non_nav_command_index == NO_COMMAND) { // If the index is NO_COMMAND then we have just loaded a nav command
non_nav_command_index = old_index + 1;
//gcs_send_text_fmt(PSTR("Non-Nav command index #%i"),non_nav_command_index);
} else if (non_nav_command_ID == NO_COMMAND) { // If the ID is NO_COMMAND then we have just completed a non-nav command
non_nav_command_index++;
}
//gcs_send_text_fmt(PSTR("Nav command index #%i"),nav_command_index);
//gcs_send_text_fmt(PSTR("Non-Nav command index #%i"),non_nav_command_index);
//gcs_send_text_fmt(PSTR("Non-Nav command ID #%i"),non_nav_command_ID);
if(nav_command_index <= (int)g.command_total && non_nav_command_ID == NO_COMMAND) {
temp = get_cmd_with_index(non_nav_command_index);
if(temp.id <= MAV_CMD_NAV_LAST) { // The next command is a nav command. No non-nav commands to do
g.command_index.set_and_save(nav_command_index);
non_nav_command_index = nav_command_index;
non_nav_command_ID = WAIT_COMMAND;
gcs_send_text_fmt(PSTR("Non-Nav command ID updated to #%i"),non_nav_command_ID);
} else { // The next command is a non-nav command. Prepare to execute it.
g.command_index.set_and_save(non_nav_command_index);
next_nonnav_command = temp;
non_nav_command_ID = next_nonnav_command.id;
gcs_send_text_fmt(PSTR("Non-Nav command ID updated to #%i"),non_nav_command_ID);
process_non_nav_command();
}
}
}
/**************************************************/
// These functions implement the commands.
/**************************************************/
static void process_nav_cmd()
{
//gcs_send_text_P(SEVERITY_LOW,PSTR("New nav command loaded"));
// clear non-nav command ID and index
non_nav_command_index = NO_COMMAND; // Redundant - remove?
non_nav_command_ID = NO_COMMAND; // Redundant - remove?
handle_process_nav_cmd();
if(control_mode == AUTO) {
if(home_is_set == true && mission.num_commands() > 1) {
mission.update();
}
}
}
static void process_non_nav_command()
{
//gcs_send_text_P(SEVERITY_LOW,PSTR("new non-nav command loaded"));
if(non_nav_command_ID < MAV_CMD_CONDITION_LAST) {
handle_process_condition_command();
} else {
handle_process_do_command();
// flag command ID so a new one is loaded
// -----------------------------------------
non_nav_command_ID = NO_COMMAND;
}
}
APMrover2/compat.h
View file @ 62e7778b
......@@ -2,9 +2,6 @@
#ifndef __COMPAT_H__
#define __COMPAT_H__
#define OUTPUT GPIO_OUTPUT
#define INPUT GPIO_INPUT
#define HIGH 1
#define LOW 0
......
APMrover2/config.h
View file @ 62e7778b
......@@ -49,51 +49,50 @@
#define DISABLED 0
//////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////
// HARDWARE CONFIGURATION AND CONNECTIONS
//////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////
// sensor types
#define CONFIG_BARO HAL_BARO_DEFAULT
#define CONFIG_COMPASS HAL_COMPASS_DEFAULT
#ifdef HAL_SERIAL0_BAUD_DEFAULT
# define SERIAL0_BAUD HAL_SERIAL0_BAUD_DEFAULT
#endif
//////////////////////////////////////////////////////////////////////////////
// LED and IO Pins
//
// HIL_MODE OPTIONAL
#ifndef HIL_MODE
#define HIL_MODE HIL_MODE_DISABLED
#endif
#if HIL_MODE != HIL_MODE_DISABLED // we are in HIL mode
#undef CONFIG_BARO
#define CONFIG_BARO HAL_BARO_HIL
#undef CONFIG_COMPASS
#define CONFIG_COMPASS HAL_COMPASS_HIL
#endif
#if CONFIG_HAL_BOARD == HAL_BOARD_APM1
# define CONFIG_INS_TYPE CONFIG_INS_OILPAN
# define CONFIG_COMPASS AP_COMPASS_HMC5843
# define BATTERY_PIN_1 0
# define CURRENT_PIN_1 1
#elif CONFIG_HAL_BOARD == HAL_BOARD_APM2
# define CONFIG_INS_TYPE CONFIG_INS_MPU6000
# define CONFIG_COMPASS AP_COMPASS_HMC5843
# define BATTERY_PIN_1 1
# define CURRENT_PIN_1 2
#elif CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL
# define CONFIG_INS_TYPE CONFIG_INS_HIL
# define CONFIG_COMPASS AP_COMPASS_HIL
# define BATTERY_PIN_1 1
# define CURRENT_PIN_1 2
#elif CONFIG_HAL_BOARD == HAL_BOARD_PX4
# define CONFIG_INS_TYPE CONFIG_INS_PX4
# define CONFIG_COMPASS AP_COMPASS_PX4
# define BATTERY_PIN_1 -1
# define CURRENT_PIN_1 -1
#elif CONFIG_HAL_BOARD == HAL_BOARD_FLYMAPLE
# define CONFIG_INS_TYPE CONFIG_INS_FLYMAPLE
# define CONFIG_COMPASS AP_COMPASS_HMC5843
# define BATTERY_PIN_1 20
# define CURRENT_PIN_1 19
#elif CONFIG_HAL_BOARD == HAL_BOARD_LINUX
# define CONFIG_INS_TYPE CONFIG_INS_L3G4200D
# define CONFIG_COMPASS AP_COMPASS_HMC5843
# define BATTERY_PIN_1 -1
# define CURRENT_PIN_1 -1
#endif
//////////////////////////////////////////////////////////////////////////////
// IMU Selection
//
#ifndef CONFIG_INS_TYPE
# define CONFIG_INS_TYPE CONFIG_INS_OILPAN
#elif CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
# define BATTERY_PIN_1 -1
# define CURRENT_PIN_1 -1
#endif
//////////////////////////////////////////////////////////////////////////////
......@@ -103,25 +102,6 @@
#define HIL_MODE HIL_MODE_DISABLED
#endif
#if HIL_MODE != HIL_MODE_DISABLED // we are in HIL mode
#undef GPS_PROTOCOL
#define GPS_PROTOCOL GPS_PROTOCOL_HIL
#undef CONFIG_INS_TYPE
#define CONFIG_INS_TYPE CONFIG_INS_HIL
#undef CONFIG_COMPASS
#define CONFIG_COMPASS AP_COMPASS_HIL
#endif
//////////////////////////////////////////////////////////////////////////////
// GPS_PROTOCOL
//
// Note that this test must follow the HIL_PROTOCOL block as the HIL
// setup may override the GPS configuration.
//
#ifndef GPS_PROTOCOL
# define GPS_PROTOCOL GPS_PROTOCOL_AUTO
#endif
#ifndef MAV_SYSTEM_ID
# define MAV_SYSTEM_ID 1
#endif
......@@ -139,6 +119,19 @@
# define SERIAL2_BAUD 57600
#endif
//////////////////////////////////////////////////////////////////////////////
// FrSky telemetry support
//
#ifndef FRSKY_TELEM_ENABLED
#if CONFIG_HAL_BOARD == HAL_BOARD_APM1 || CONFIG_HAL_BOARD == HAL_BOARD_APM2
# define FRSKY_TELEM_ENABLED DISABLED
#else
# define FRSKY_TELEM_ENABLED ENABLED
#endif
#endif
#ifndef CH7_OPTION
# define CH7_OPTION CH7_SAVE_WP
#endif
......
APMrover2/control_modes.pde
View file @ 62e7778b
......@@ -9,6 +9,11 @@ static void read_control_switch()
// If we get this value we do not want to change modes.
if(switchPosition == 255) return;
if (hal.scheduler->millis() - failsafe.last_valid_rc_ms > 100) {
// only use signals that are less than 0.1s old.
return;
}
// we look for changes in the switch position. If the
// RST_SWITCH_CH parameter is set, then it is a switch that can be
// used to force re-reading of the control switch. This is useful
......@@ -32,7 +37,7 @@ static void read_control_switch()
static uint8_t readSwitch(void){
uint16_t pulsewidth = hal.rcin->read(g.mode_channel - 1);
if (pulsewidth <= 910 || pulsewidth >= 2090) return 255; // This is an error condition
if (pulsewidth <= 900 || pulsewidth >= 2200) return 255; // This is an error condition
if (pulsewidth > 1230 && pulsewidth <= 1360) return 1;
if (pulsewidth > 1360 && pulsewidth <= 1490) return 2;
if (pulsewidth > 1490 && pulsewidth <= 1620) return 3;
......@@ -67,36 +72,28 @@ static void read_trim_switch()
if (control_mode == MANUAL) {
hal.console->println_P(PSTR("Erasing waypoints"));
// if SW7 is ON in MANUAL = Erase the Flight Plan
g.command_total.set_and_save(CH7_wp_index);
g.command_total = 0;
g.command_index =0;
nav_command_index = 0;
mission.clear();
if (channel_steer->control_in > 3000) {
// if roll is full right store the current location as home
init_home();
}
CH7_wp_index = 1;
return;
} else if (control_mode == LEARNING || control_mode == STEERING) {
// if SW7 is ON in LEARNING = record the Wp
// set the next_WP (home is stored at 0)
hal.console->printf_P(PSTR("Learning waypoint %u"), (unsigned)CH7_wp_index);
current_loc.id = MAV_CMD_NAV_WAYPOINT;
// store the index
g.command_total.set_and_save(CH7_wp_index);
g.command_total = CH7_wp_index;
g.command_index = CH7_wp_index;
nav_command_index = 0;
// save command
set_cmd_with_index(current_loc, CH7_wp_index);
// increment index
CH7_wp_index++;
CH7_wp_index = constrain_int16(CH7_wp_index, 1, MAX_WAYPOINTS);
// create new mission command
AP_Mission::Mission_Command cmd = {};
// set new waypoint to current location
cmd.content.location = current_loc;
// make the new command to a waypoint
cmd.id = MAV_CMD_NAV_WAYPOINT;
// save command
if(mission.add_cmd(cmd)) {
hal.console->printf_P(PSTR("Learning waypoint %u"), (unsigned)mission.num_commands());
}
} else if (control_mode == AUTO) {
// if SW7 is ON in AUTO = set to RTL
set_mode(RTL);
......
APMrover2/defines.h
View file @ 62e7778b
......@@ -28,21 +28,9 @@ enum ch7_option {
#define T6 1000000
#define T7 10000000
// GPS type codes - use the names, not the numbers
#define GPS_PROTOCOL_NONE -1
#define GPS_PROTOCOL_NMEA 0
#define GPS_PROTOCOL_SIRF 1
#define GPS_PROTOCOL_UBLOX 2
#define GPS_PROTOCOL_IMU 3
#define GPS_PROTOCOL_MTK 4
#define GPS_PROTOCOL_HIL 5
#define GPS_PROTOCOL_MTK19 6
#define GPS_PROTOCOL_AUTO 7
// HIL enumerations
#define HIL_MODE_DISABLED 0
#define HIL_MODE_ATTITUDE 1
#define HIL_MODE_SENSORS 2
#define HIL_MODE_SENSORS 1
// Auto Pilot modes
// ----------------
......@@ -62,15 +50,6 @@ enum mode {
#define FAILSAFE_EVENT_GCS (1<<1)
#define FAILSAFE_EVENT_RC (1<<2)
// Commands - Note that APM now uses a subset of the MAVLink protocol commands. See enum MAV_CMD in the GCS_Mavlink library
#define CMD_BLANK 0 // there is no command stored in the mem location requested
#define NO_COMMAND 0
#define WAIT_COMMAND 255
// Command/Waypoint/Location Options Bitmask
//--------------------
#define MASK_OPTIONS_RELATIVE_ALT (1<<0) // 1 = Relative altitude
//repeating events
#define NO_REPEAT 0
#define CH_5_TOGGLE 1
......@@ -86,16 +65,15 @@ enum mode {
#define LOG_CTUN_MSG 0x01
#define LOG_NTUN_MSG 0x02
#define LOG_PERFORMANCE_MSG 0x03
#define LOG_CMD_MSG 0x04
#define LOG_CURRENT_MSG 0x05
#define LOG_STARTUP_MSG 0x06
#define LOG_SONAR_MSG 0x07
#define LOG_ATTITUDE_MSG 0x08
#define LOG_MODE_MSG 0x09
#define LOG_COMPASS_MSG 0x0A
#define LOG_CAMERA_MSG 0x0B
#define LOG_COMPASS2_MSG 0x0C
#define LOG_STEERING_MSG 0x0D
#define LOG_COMPASS3_MSG 0x0E
#define TYPE_AIRSTART_MSG 0x00
#define TYPE_GROUNDSTART_MSG 0x01
......@@ -116,7 +94,7 @@ enum mode {
#define MASK_LOG_CAMERA (1<<12)
#define MASK_LOG_STEERING (1<<13)
#define MASK_LOG_RC (1<<14)
#define MASK_LOG_WHEN_DISARMED (1<<30)
#define MASK_LOG_WHEN_DISARMED (1UL<<16)
// Waypoint Modes
// ----------------
......@@ -139,9 +117,6 @@ enum mode {
// Climb rate calculations
#define ALTITUDE_HISTORY_LENGTH 8 //Number of (time,altitude) points to regress a climb rate from
#define RELAY_PIN 47
// sonar
#define MAX_SONAR_XL 0
#define MAX_SONAR_LV 1
......@@ -151,32 +126,16 @@ enum mode {
#define SPEEDFILT 400 // centimeters/second; the speed below which a groundstart will be triggered
// EEPROM addresses
#define EEPROM_MAX_ADDR 4096
// parameters get the first 1KiB of EEPROM, remainder is for waypoints
#define WP_START_BYTE 0x500 // where in memory home WP is stored + all other WP
#define WP_SIZE 15
#define MAX_WAYPOINTS ((EEPROM_MAX_ADDR - WP_START_BYTE) / WP_SIZE) - 1 // - 1 to be safe
// convert a boolean (0 or 1) to a sign for multiplying (0 maps to 1, 1 maps to -1)
#define BOOL_TO_SIGN(bvalue) ((bvalue)?-1:1)
// 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
// compass driver types
#define AP_COMPASS_HMC5843 1
#define AP_COMPASS_PX4 2
#define AP_COMPASS_HIL 3
enum Serial2Protocol {
SERIAL2_MAVLINK = 1,
SERIAL2_FRSKY_DPORT = 2,
SERIAL2_FRSKY_SPORT = 3 // not supported yet
};
#endif // _DEFINES_H
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()
......@@ -26,35 +30,24 @@ static void init_rc_in()
static void init_rc_out()
{
for (uint8_t i=0; i<8; i++) {
RC_Channel::rc_channel(i)->enable_out();
RC_Channel::rc_channel(i)->output_trim();
}
RC_Channel::rc_channel(CH_1)->enable_out();
RC_Channel::rc_channel(CH_3)->enable_out();
RC_Channel::output_trim_all();
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
servo_write(CH_9, g.rc_9.radio_trim);
#endif
#if CONFIG_HAL_BOARD == HAL_BOARD_APM2 || CONFIG_HAL_BOARD == HAL_BOARD_PX4
servo_write(CH_10, g.rc_10.radio_trim);
servo_write(CH_11, g.rc_11.radio_trim);
#endif
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
servo_write(CH_12, g.rc_12.radio_trim);
#endif
// setup PWM values to send if the FMU firmware dies
RC_Channel::setup_failsafe_trim_all();
}
static void read_radio()
{
if (!hal.rcin->valid_channels()) {
if (!hal.rcin->new_input()) {
control_failsafe(channel_throttle->radio_in);
return;
}
failsafe.last_valid_rc_ms = hal.scheduler->millis();
for (uint8_t i=0; i<8; i++) {
RC_Channel::rc_channel(i)->set_pwm(RC_Channel::rc_channel(i)->read());
}
RC_Channel::set_pwm_all();
control_failsafe(channel_throttle->radio_in);
......
APMrover2/release-notes.txt 0 → 100644
View file @ 62e7778b
Release 2.47, November 15th 2014
--------------------------------
The ardupilot development team is proud to announce the release of
version 2.47 of APM:Rover. This is a minor bug fix release. The most
important change in this release is the fixing of the skid steering
support but there have been a number of fixes in other areas as well.
Full changes list for this release:
- add support for controlling safety switch on Pixhawk from ground station
- prevent reports of failed AHRS during initialisation
- fixed skid steering that was broken in the last release
- report gyro unhealthy if gyro calibration failed
- fixed dual sonar support in CLI sonar test
- fixed Nuttx crash on Pixhawk with bad I2C cables
- added GPS_SBAS_MODE parameter - turns on/off satellite based augemtation system for GPS
- added GPS_MIN_ELEV parameter - specifiy the elevation mask for GPS satellites
- added RELAY_DEFAULT parameter to control default of relay on startup
- fixed bug in FRAM storage on Pixhawk that could cause parameters changes not to be saved
- better handling of compass errors in the EKF (Extended Kalman Filter)
- improved support for linux based autopilots
- added support for PulsedLight LIDAR as a range finder
Many thanks to everyone who contributed to this release, especially
Tom Coyle and Linus Penzlien for their excellent testing and feedback.
Happy driving!
Release 2.46, August 26th 2014
------------------------------
The ardupilot development team is proud to announce the release of
version 2.46 of APM:Rover. This is a major release with a lot of new
features and bug fixes.
This release is based on a lot of development and testing that
happened prior to the AVC competition where APM based vehicles
performed very well.
Full changes list for this release:
- added support for higher baudrates on telemetry ports, to make it
easier to use high rate telemetry to companion boards. Rates of up
to 1.5MBit are now supported to companion boards.
- new Rangefinder code with support for a wider range of rangefinder
types including a range of Lidars (thanks to Allyson Kreft)
- added logging of power status on Pixhawk
- added PIVOT_TURN_ANGLE parameter for pivot based turns on skid
steering rovers
- lots of improvements to the EKF support for Rover, thanks to Paul
Riseborough and testing from Tom Coyle. Using the EKF can greatly
improve navigation accuracy for fast rovers. Enable with
AHRS_EKF_USE=1.
- improved support for dual GPS on Pixhawk. Using a 2nd GPS can
greatly improve performance when in an area with an obstructed
view of the sky
- support for up to 14 RC channels on Pihxawk
- added BRAKING_PERCENT and BRAKING_SPEEDERR parameters for better
breaking support when cornering
- added support for FrSky telemetry via SERIAL2_PROTOCOL parameter
(thanks to Matthias Badaire)
- added support for Linux based autopilots, initially with the PXF
BeagleBoneBlack cape and the Erle robotics board. Support for more
boards is expected in future releases. Thanks to Victor, Sid and
Anuj for their great work on the Linux port.
- added StorageManager library, which expands available FRAM storage
on Pixhawk to 16 kByte. This allows for 724 waypoints, 50 rally
points and 84 fence points on Pixhawk.
- improved reporting of magnetometer and barometer errors to the GCS
- fixed a bug in automatic flow control detection for serial ports
in Pixhawk
- fixed use of FMU servo pins as digital inputs on Pixhawk
- imported latest updates for VRBrain boards (thanks to Emile
Castelnuovo and Luca Micheletti)
- updates to the Piksi GPS support (thanks to Niels Joubert)
- improved gyro estimate in DCM (thanks to Jon Challinger)
- improved position projection in DCM in wind (thanks to Przemek
Lekston)
- several updates to AP_NavEKF for more robust handling of errors
(thanks to Paul Riseborough)
- lots of small code cleanups thanks to Daniel Frenzel
- initial support for NavIO board from Mikhail Avkhimenia
- fixed logging of RCOU for up to 12 channels (thanks to Emile
Castelnuovo)
- code cleanups from Silvia Nunezrivero
- improved parameter download speed on radio links with no flow
control
Many thanks to everyone who contributed to this release, especially
Tom Coyle and Linus Penzlien for their excellent testing and feedback.
Happy driving!
APMrover2/sensors.pde
View file @ 62e7778b
// -*- 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"));
}
static void init_sonar(void)
{
#if CONFIG_HAL_BOARD == HAL_BOARD_APM1
sonar.Init(&adc);
sonar2.Init(&adc);
#else
sonar.Init(NULL);
sonar2.Init(NULL);
#endif
sonar.init();
}
// read_battery - reads battery voltage and current and invokes failsafe
......@@ -30,15 +31,17 @@ void read_receiver_rssi(void)
// read the sonars
static void read_sonars(void)
{
if (!sonar.enabled()) {
sonar.update();
if (!sonar.healthy()) {
// this makes it possible to disable sonar at runtime
return;
}
if (sonar2.enabled()) {
if (sonar.healthy(1)) {
// we have two sonars
obstacle.sonar1_distance_cm = sonar.distance_cm();
obstacle.sonar2_distance_cm = sonar2.distance_cm();
obstacle.sonar1_distance_cm = sonar.distance_cm(0);
obstacle.sonar2_distance_cm = sonar.distance_cm(1);
if (obstacle.sonar1_distance_cm <= (uint16_t)g.sonar_trigger_cm &&
obstacle.sonar2_distance_cm <= (uint16_t)obstacle.sonar2_distance_cm) {
// we have an object on the left
......@@ -65,7 +68,7 @@ static void read_sonars(void)
}
} else {
// we have a single sonar
obstacle.sonar1_distance_cm = sonar.distance_cm();
obstacle.sonar1_distance_cm = sonar.distance_cm(0);
obstacle.sonar2_distance_cm = 0;
if (obstacle.sonar1_distance_cm <= (uint16_t)g.sonar_trigger_cm) {
// obstacle detected in front
......