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 (5055)
4,955 additional commits have been omitted to prevent performance issues.
Expand all Hide whitespace changes
Inline Side-by-side
Showing
with 1269 additions and 2368 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
......@@ -2,23 +2,30 @@
*.bin
*.d
*.dll
*.elf
*.hex
*.dfu
*.exe
*.lst
*.o
*.obj
*.px4
*.vrx
*.pyc
*.tlog
*.tlog.raw
*.vbrain
*.zip
.*.swo
.*.swp
.built
.context
.cproject
.depend
.directory
.DS_Store
.metadata/
.project
.settings/
.vagrant
/tmp/*
......@@ -40,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
......@@ -61,3 +79,4 @@ serialsent.raw
status.txt
tags
test.ArduCopter/*
Thumbs.db
.project deleted 100644 → 0
View file @ 8ea3e686
<?xml version="1.0" encoding="UTF-8"?>
<projectDescription>
<name>ArduPilotMega-Source@ardupilotone</name>
<comment></comment>
<projects>
</projects>
<buildSpec>
</buildSpec>
<natures>
</natures>
</projectDescription>
.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
This diff is collapsed.
APMrover2/GCS.h deleted 100644 → 0
View file @ 8ea3e686
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
/// @file GCS.h
/// @brief Interface definition for the various Ground Control System
// protocols.
#ifndef __GCS_H
#define __GCS_H
#include <AP_HAL.h>
#include <AP_Common.h>
#include <GPS.h>
#include <stdint.h>
///
/// @class GCS
/// @brief Class describing the interface between the APM code
/// proper and the GCS implementation.
///
/// GCS' are currently implemented inside the sketch and as such have
/// access to all global state. The sketch should not, however, call GCS
/// internal functions - all calls to the GCS should be routed through
/// this interface (or functions explicitly exposed by a subclass).
///
class GCS_Class
{
public:
/// Startup initialisation.
///
/// This routine performs any one-off initialisation required before
/// GCS messages are exchanged.
///
/// @note The stream is expected to be set up and configured for the
/// correct bitrate before ::init is called.
///
/// @note The stream is currently BetterStream so that we can use the _P
/// methods; this may change if Arduino adds them to Print.
///
/// @param port The stream over which messages are exchanged.
///
void init(AP_HAL::UARTDriver *port) {
_port = port;
initialised = true;
}
/// Update GCS state.
///
/// This may involve checking for received bytes on the stream,
/// or sending additional periodic messages.
void update(void) {
}
/// Send a message with a single numeric parameter.
///
/// This may be a standalone message, or the GCS driver may
/// have its own way of locating additional parameters to send.
///
/// @param id ID of the message to send.
/// @param param Explicit message parameter.
///
void send_message(enum ap_message id) {
}
/// Send a text message.
///
/// @param severity A value describing the importance of the message.
/// @param str The text to be sent.
///
void send_text(gcs_severity severity, const char *str) {
}
/// Send a text message with a PSTR()
///
/// @param severity A value describing the importance of the message.
/// @param str The text to be sent.
///
void send_text_P(gcs_severity severity, const prog_char_t *str) {
}
// send streams which match frequency range
void data_stream_send(void);
// set to true if this GCS link is active
bool initialised;
protected:
/// The stream we are communicating over
AP_HAL::UARTDriver * _port;
};
//
// GCS class definitions.
//
// These are here so that we can declare the GCS object early in the sketch
// and then reference it statically rather than via a pointer.
//
///
/// @class GCS_MAVLINK
/// @brief The mavlink protocol for qgroundcontrol
///
class GCS_MAVLINK : public GCS_Class
{
public:
GCS_MAVLINK();
void update(void);
void init(AP_HAL::UARTDriver *port);
void send_message(enum ap_message id);
void send_text(gcs_severity severity, const char *str);
void send_text_P(gcs_severity severity, const prog_char_t *str);
void data_stream_send(void);
void queued_param_send();
void queued_waypoint_send();
static const struct AP_Param::GroupInfo var_info[];
// NOTE! The streams enum below and the
// set of AP_Int16 stream rates _must_ be
// kept in the same order
enum streams {STREAM_RAW_SENSORS,
STREAM_EXTENDED_STATUS,
STREAM_RC_CHANNELS,
STREAM_RAW_CONTROLLER,
STREAM_POSITION,
STREAM_EXTRA1,
STREAM_EXTRA2,
STREAM_EXTRA3,
STREAM_PARAMS,
NUM_STREAMS};
// see if we should send a stream now. Called at 50Hz
bool stream_trigger(enum streams stream_num);
// this costs us 51 bytes per instance, but means that low priority
// messages don't block the CPU
mavlink_statustext_t pending_status;
// call to reset the timeout window for entering the cli
void reset_cli_timeout();
private:
void handleMessage(mavlink_message_t * msg);
/// Perform queued sending operations
///
AP_Param * _queued_parameter; ///< next parameter to
// be sent in queue
enum ap_var_type _queued_parameter_type; ///< type of the next
// parameter
AP_Param::ParamToken _queued_parameter_token; ///AP_Param token for
// next() call
uint16_t _queued_parameter_index; ///< next queued
// parameter's index
uint16_t _queued_parameter_count; ///< saved count of
// parameters for
// queued send
uint32_t _queued_parameter_send_time_ms;
/// Count the number of reportable parameters.
///
/// Not all parameters can be reported via MAVlink. We count the number
// that are
/// so that we can report to a GCS the number of parameters it should
// expect when it
/// requests the full set.
///
/// @return The number of reportable parameters.
///
uint16_t _count_parameters(); ///< count reportable
// parameters
uint16_t _parameter_count; ///< cache of reportable
// parameters
mavlink_channel_t chan;
uint16_t packet_drops;
#if CLI_ENABLED == ENABLED
// this allows us to detect the user wanting the CLI to start
uint8_t crlf_count;
#endif
// waypoints
uint16_t waypoint_request_i; // request index
uint16_t waypoint_request_last; // last request index
uint16_t waypoint_dest_sysid; // where to send requests
uint16_t waypoint_dest_compid; // "
bool waypoint_receiving; // currently receiving
uint16_t waypoint_count;
uint32_t waypoint_timelast_send; // milliseconds
uint32_t waypoint_timelast_receive; // milliseconds
uint32_t waypoint_timelast_request; // milliseconds
uint16_t waypoint_send_timeout; // milliseconds
uint16_t waypoint_receive_timeout; // milliseconds
// saveable rate of each stream
AP_Int16 streamRates[NUM_STREAMS];
// number of 50Hz ticks until we next send this stream
uint8_t stream_ticks[NUM_STREAMS];
// number of extra ticks to add to slow things down for the radio
uint8_t stream_slowdown;
// millis value to calculate cli timeout relative to.
// exists so we can separate the cli entry time from the system start time
uint32_t _cli_timeout;
};
#endif // __GCS_H
APMrover2/GCS_Mavlink.pde
View file @ 62e7778b
This diff is collapsed.
APMrover2/Log.pde
View file @ 62e7778b
......@@ -51,6 +51,7 @@ print_log_menu(void)
PLOG(SONAR);
PLOG(COMPASS);
PLOG(CAMERA);
PLOG(STEERING);
#undef PLOG
}
......@@ -143,6 +144,7 @@ select_logs(uint8_t argc, const Menu::arg *argv)
TARG(SONAR);
TARG(COMPASS);
TARG(CAMERA);
TARG(STEERING);
#undef TARG
}
......@@ -163,17 +165,15 @@ process_logs(uint8_t argc, const Menu::arg *argv)
struct PACKED log_Performance {
LOG_PACKET_HEADER;
uint32_t time_ms;
uint32_t loop_time;
uint16_t main_loop_count;
int16_t g_dt_max;
uint8_t renorm_count;
uint8_t renorm_blowup;
uint8_t gps_fix_count;
uint32_t g_dt_max;
int16_t gyro_drift_x;
int16_t gyro_drift_y;
int16_t gyro_drift_z;
int16_t pm_test;
uint8_t i2c_lockup_count;
uint16_t ins_error_count;
};
// Write a performance monitoring packet. Total length : 19 bytes
......@@ -181,102 +181,75 @@ static void Log_Write_Performance()
{
struct log_Performance pkt = {
LOG_PACKET_HEADER_INIT(LOG_PERFORMANCE_MSG),
time_ms : millis(),
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,
gps_fix_count : gps_fix_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),
pm_test : pmTest1,
i2c_lockup_count: hal.i2c->lockup_count()
i2c_lockup_count: hal.i2c->lockup_count(),
ins_error_count : ins.error_count()
};
DataFlash.WriteBlock(&pkt, sizeof(pkt));
}
struct PACKED log_Cmd {
LOG_PACKET_HEADER;
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),
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));
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_Camera {
struct PACKED log_Steering {
LOG_PACKET_HEADER;
uint32_t gps_time;
int32_t latitude;
int32_t longitude;
int16_t roll;
int16_t pitch;
uint16_t yaw;
uint32_t time_ms;
float demanded_accel;
float achieved_accel;
};
// Write a Camera packet. Total length : 26 bytes
static void Log_Write_Camera()
// Write a steering packet
static void Log_Write_Steering()
{
#if CAMERA == ENABLED
struct log_Camera pkt = {
LOG_PACKET_HEADER_INIT(LOG_CAMERA_MSG),
gps_time : g_gps->time,
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
struct log_Steering pkt = {
LOG_PACKET_HEADER_INIT(LOG_STEERING_MSG),
time_ms : hal.scheduler->millis(),
demanded_accel : lateral_acceleration,
achieved_accel : gps.ground_speed() * ins.get_gyro().z,
};
DataFlash.WriteBlock(&pkt, sizeof(pkt));
#endif
}
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)
{
struct log_Startup pkt = {
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);
}
}
}
struct PACKED log_Control_Tuning {
LOG_PACKET_HEADER;
uint32_t time_ms;
int16_t steer_out;
int16_t roll;
int16_t pitch;
......@@ -290,6 +263,7 @@ static void Log_Write_Control_Tuning()
Vector3f accel = ins.get_accel();
struct log_Control_Tuning pkt = {
LOG_PACKET_HEADER_INIT(LOG_CTUN_MSG),
time_ms : millis(),
steer_out : (int16_t)channel_steer->servo_out,
roll : (int16_t)ahrs.roll_sensor,
pitch : (int16_t)ahrs.pitch_sensor,
......@@ -301,6 +275,7 @@ static void Log_Write_Control_Tuning()
struct PACKED log_Nav_Tuning {
LOG_PACKET_HEADER;
uint32_t time_ms;
uint16_t yaw;
float wp_distance;
uint16_t target_bearing_cd;
......@@ -313,6 +288,7 @@ static void Log_Write_Nav_Tuning()
{
struct log_Nav_Tuning pkt = {
LOG_PACKET_HEADER_INIT(LOG_NTUN_MSG),
time_ms : millis(),
yaw : (uint16_t)ahrs.yaw_sensor,
wp_distance : wp_distance,
target_bearing_cd : (uint16_t)nav_controller->target_bearing_cd(),
......@@ -324,6 +300,7 @@ static void Log_Write_Nav_Tuning()
struct PACKED log_Attitude {
LOG_PACKET_HEADER;
uint32_t time_ms;
int16_t roll;
int16_t pitch;
uint16_t yaw;
......@@ -335,24 +312,35 @@ static void Log_Write_Attitude()
{
struct log_Attitude pkt = {
LOG_PACKET_HEADER_INIT(LOG_ATTITUDE_MSG),
roll : (int16_t)ahrs.roll_sensor,
pitch : (int16_t)ahrs.pitch_sensor,
yaw : (uint16_t)ahrs.yaw_sensor
time_ms : millis(),
roll : (int16_t)ahrs.roll_sensor,
pitch : (int16_t)ahrs.pitch_sensor,
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 log_Mode {
struct PACKED log_Mode {
LOG_PACKET_HEADER;
uint32_t time_ms;
uint8_t mode;
uint8_t mode_num;
};
// Write a mode packet. Total length : 7 bytes
// Write a mode packet
static void Log_Write_Mode()
{
struct log_Mode pkt = {
LOG_PACKET_HEADER_INIT(LOG_MODE_MSG),
time_ms : millis(),
mode : (uint8_t)control_mode,
mode_num : (uint8_t)control_mode
};
......@@ -362,6 +350,7 @@ static void Log_Write_Mode()
struct PACKED log_Sonar {
LOG_PACKET_HEADER;
uint32_t time_ms;
float lateral_accel;
uint16_t sonar1_distance;
uint16_t sonar2_distance;
......@@ -381,9 +370,10 @@ static void Log_Write_Sonar()
}
struct log_Sonar pkt = {
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,
......@@ -395,6 +385,7 @@ static void Log_Write_Sonar()
struct PACKED log_Current {
LOG_PACKET_HEADER;
uint32_t time_ms;
int16_t throttle_in;
int16_t battery_voltage;
int16_t current_amps;
......@@ -406,17 +397,22 @@ static void Log_Write_Current()
{
struct log_Current pkt = {
LOG_PACKET_HEADER_INIT(LOG_CURRENT_MSG),
time_ms : millis(),
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 {
LOG_PACKET_HEADER;
uint32_t time_ms;
int16_t mag_x;
int16_t mag_y;
int16_t mag_z;
......@@ -431,13 +427,15 @@ struct PACKED log_Compass {
// Write a Compass packet. Total length : 15 bytes
static void Log_Write_Compass()
{
Vector3f mag_offsets = compass.get_offsets();
Vector3f mag_motor_offsets = compass.get_motor_offsets();
const Vector3f &mag_offsets = compass.get_offsets();
const Vector3f &mag_motor_offsets = compass.get_motor_offsets();
const Vector3f &mag = compass.get_field();
struct log_Compass pkt = {
LOG_PACKET_HEADER_INIT(LOG_COMPASS_MSG),
mag_x : compass.mag_x,
mag_y : compass.mag_y,
mag_z : compass.mag_z,
time_ms : millis(),
mag_x : (int16_t)mag.x,
mag_y : (int16_t)mag.y,
mag_z : (int16_t)mag.z,
offset_x : (int16_t)mag_offsets.x,
offset_y : (int16_t)mag_offsets.y,
offset_z : (int16_t)mag_offsets.z,
......@@ -446,48 +444,99 @@ static void Log_Write_Compass()
motor_offset_z : (int16_t)mag_motor_offsets.z
};
DataFlash.WriteBlock(&pkt, sizeof(pkt));
#if COMPASS_MAX_INSTANCES > 1
if (compass.get_count() > 1) {
const Vector3f &mag2_offsets = compass.get_offsets(1);
const Vector3f &mag2_motor_offsets = compass.get_motor_offsets(1);
const Vector3f &mag2 = compass.get_field(1);
struct log_Compass pkt2 = {
LOG_PACKET_HEADER_INIT(LOG_COMPASS2_MSG),
time_ms : millis(),
mag_x : (int16_t)mag2.x,
mag_y : (int16_t)mag2.y,
mag_z : (int16_t)mag2.z,
offset_x : (int16_t)mag2_offsets.x,
offset_y : (int16_t)mag2_offsets.y,
offset_z : (int16_t)mag2_offsets.z,
motor_offset_x : (int16_t)mag2_motor_offsets.x,
motor_offset_y : (int16_t)mag2_motor_offsets.y,
motor_offset_z : (int16_t)mag2_motor_offsets.z
};
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
}
static void Log_Write_RC(void)
{
DataFlash.Log_Write_RCIN();
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", "ccC", "Roll,Pitch,Yaw" },
"ATT", "IccC", "TimeMS,Roll,Pitch,Yaw" },
{ LOG_PERFORMANCE_MSG, sizeof(log_Performance),
"PM", "IHhBBBhhhhB", "LTime,MLC,gDt,RNCnt,RNBl,GPScnt,GDx,GDy,GDz,PMT,I2CErr" },
{ LOG_CMD_MSG, sizeof(log_Cmd),
"CMD", "BBBBBeLL", "CTot,CNum,CId,COpt,Prm1,Alt,Lat,Lng" },
{ LOG_CAMERA_MSG, sizeof(log_Camera),
"CAM", "ILLccC", "GPSTime,Lat,Lng,Roll,Pitch,Yaw" },
"PM", "IIHIhhhBH", "TimeMS,LTime,MLC,gDt,GDx,GDy,GDz,I2CErr,INSErr" },
{ LOG_STARTUP_MSG, sizeof(log_Startup),
"STRT", "BB", "SType,CTot" },
"STRT", "IBH", "TimeMS,SType,CTot" },
{ LOG_CTUN_MSG, sizeof(log_Control_Tuning),
"CTUN", "hcchf", "Steer,Roll,Pitch,ThrOut,AccY" },
"CTUN", "Ihcchf", "TimeMS,Steer,Roll,Pitch,ThrOut,AccY" },
{ LOG_NTUN_MSG, sizeof(log_Nav_Tuning),
"NTUN", "HfHHb", "Yaw,WpDist,TargBrg,NavBrg,Thr" },
"NTUN", "IHfHHb", "TimeMS,Yaw,WpDist,TargBrg,NavBrg,Thr" },
{ LOG_SONAR_MSG, sizeof(log_Sonar),
"SONR", "fHHHbHCb", "LatAcc,S1Dist,S2Dist,DCnt,TAng,TTim,Spd,Thr" },
"SONR", "IfHHHbHCb", "TimeMS,LatAcc,S1Dist,S2Dist,DCnt,TAng,TTim,Spd,Thr" },
{ LOG_CURRENT_MSG, sizeof(log_Current),
"CURR", "hhhHf", "Thr,Volt,Curr,Vcc,CurrTot" },
"CURR", "IhhhHf", "TimeMS,Thr,Volt,Curr,Vcc,CurrTot" },
{ LOG_MODE_MSG, sizeof(log_Mode),
"MODE", "MB", "Mode,ModeNum" },
"MODE", "IMB", "TimeMS,Mode,ModeNum" },
{ LOG_COMPASS_MSG, sizeof(log_Compass),
"MAG", "hhhhhhhhh", "MagX,MagY,MagZ,OfsX,OfsY,OfsZ,MOfsX,MOfsY,MOfsZ" },
"MAG", "Ihhhhhhhhh", "TimeMS,MagX,MagY,MagZ,OfsX,OfsY,OfsZ,MOfsX,MOfsY,MOfsZ" },
{ LOG_COMPASS2_MSG, sizeof(log_Compass),
"MAG2", "Ihhhhhhhhh", "TimeMS,MagX,MagY,MagZ,OfsX,OfsY,OfsZ,MOfsX,MOfsY,MOfsZ" },
{ LOG_STEERING_MSG, sizeof(log_Steering),
"STER", "Iff", "TimeMS,Demanded,Achieved" },
};
// Read the DataFlash log memory : Packet Parser
static void Log_Read(uint16_t log_num, uint16_t start_page, uint16_t end_page)
{
cliSerial->printf_P(PSTR("\n" THISFIRMWARE
cliSerial->printf_P(PSTR("\n" FIRMWARE_STRING
"\nFree RAM: %u\n"),
(unsigned) memcheck_available_memory());
(unsigned)hal.util->available_memory());
cliSerial->println_P(PSTR(HAL_BOARD_NAME));
DataFlash.LogReadProcess(log_num, start_page, end_page,
sizeof(log_structure)/sizeof(log_structure[0]),
log_structure,
print_mode,
cliSerial);
}
......@@ -495,17 +544,30 @@ static void Log_Read(uint16_t log_num, uint16_t start_page, uint16_t end_page)
// start a new log
static void start_logging()
{
DataFlash.StartNewLog(sizeof(log_structure)/sizeof(log_structure[0]), log_structure);
in_mavlink_delay = true;
DataFlash.StartNewLog();
in_mavlink_delay = false;
DataFlash.Log_Write_Message_P(PSTR(FIRMWARE_STRING));
#if defined(PX4_GIT_VERSION) && defined(NUTTX_GIT_VERSION)
DataFlash.Log_Write_Message_P(PSTR("PX4: " PX4_GIT_VERSION " NuttX: " NUTTX_GIT_VERSION));
#endif
// write system identifier as well if available
char sysid[40];
if (hal.util->get_system_id(sysid)) {
DataFlash.Log_Write_Message(sysid);
}
}
#else // LOGGING_ENABLED
// 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() {}
......@@ -513,6 +575,7 @@ static void Log_Write_Mode() {}
static void Log_Write_Attitude() {}
static void Log_Write_Compass() {}
static void start_logging() {}
static void Log_Write_RC(void) {}
#endif // LOGGING_ENABLED
APMrover2/Parameters.h
View file @ 62e7778b
......@@ -28,35 +28,55 @@ 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
//
k_param_gcs0 = 110, // stream rates for port0
k_param_gcs3, // stream rates for port3
k_param_gcs0 = 110, // stream rates for uartA
k_param_gcs1, // stream rates for uartC
k_param_sysid_this_mav,
k_param_sysid_my_gcs,
k_param_serial0_baud,
k_param_serial3_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_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
......@@ -110,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
......@@ -132,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,
//
......@@ -163,6 +184,7 @@ public:
k_param_rcmap,
k_param_L1_controller,
k_param_steerController,
k_param_barometer,
// 254,255: reserved
};
......@@ -172,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;
......@@ -180,12 +202,20 @@ 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 serial3_baud;
AP_Int16 serial0_baud;
AP_Int16 serial1_baud;
#if MAVLINK_COMM_NUM_BUFFERS > 2
AP_Int16 serial2_baud;
AP_Int8 serial2_protocol;
#endif
AP_Int8 telem_delay;
AP_Int8 skip_gyro_cal;
......@@ -201,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;
......@@ -211,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
......@@ -258,8 +291,6 @@ public:
// Waypoints
//
AP_Int8 command_total;
AP_Int8 command_index;
AP_Float waypoint_radius;
// PID controllers
......@@ -276,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} }
#define GGROUP(v, name, class) { AP_PARAM_GROUP, name, Parameters::k_param_ ## v, &g.v, {group_info:class::var_info} }
#define GOBJECT(v, name, class) { AP_PARAM_GROUP, name, Parameters::k_param_ ## v, &v, {group_info:class::var_info} }
#define GOBJECTN(v, pname, name, class) { AP_PARAM_GROUP, name, Parameters::k_param_ ## pname, &v, {group_info : class::var_info} }
const AP_Param::Info var_info[] PROGMEM = {
GSCALAR(format_version, "FORMAT_VERSION", 1),
......@@ -37,35 +38,57 @@ 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),
// @Param: SYSID_THIS_MAV
// @DisplayName: MAVLink system ID
// @Description: ID used in MAVLink protocol to identify this vehicle
// @Range: 1 255
// @User: Advanced
GSCALAR(sysid_this_mav, "SYSID_THISMAV", MAV_SYSTEM_ID),
// @Param: SYSID_MYGCS
// @DisplayName: MAVLink ground station ID
// @Description: ID used in MAVLink protocol to identify the controlling ground station
// @Range: 1 255
// @User: Advanced
GSCALAR(sysid_my_gcs, "SYSID_MYGCS", 255),
// @Param: SERIAL0_BAUD
// @DisplayName: USB Console Baud Rate
// @Description: The baud rate used on the first serial 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 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", SERIAL0_BAUD/1000),
GSCALAR(serial0_baud, "SERIAL0_BAUD", SERIAL0_BAUD/1000),
// @Param: SERIAL3_BAUD
// @Param: SERIAL1_BAUD
// @DisplayName: Telemetry Baud Rate
// @Description: The baud rate used on the telemetry port
// @Values: 1:1200,2:2400,4:4800,9:9600,19:19200,38:38400,57:57600,111:111100,115:115200
// @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(serial3_baud, "SERIAL3_BAUD", SERIAL3_BAUD/1000),
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. 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", 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
......@@ -133,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
......@@ -153,32 +203,32 @@ const AP_Param::Info var_info[] PROGMEM = {
GGROUP(rc_3, "RC3_", RC_Channel),
// @Group: RC4_
// @Path: ../libraries/RC_Channel/RC_Channel.cpp
// @Path: ../libraries/RC_Channel/RC_Channel.cpp,../libraries/RC_Channel/RC_Channel_aux.cpp
GGROUP(rc_4, "RC4_", RC_Channel_aux),
// @Group: RC5_
// @Path: ../libraries/RC_Channel/RC_Channel.cpp
// @Path: ../libraries/RC_Channel/RC_Channel.cpp,../libraries/RC_Channel/RC_Channel_aux.cpp
GGROUP(rc_5, "RC5_", RC_Channel_aux),
// @Group: RC6_
// @Path: ../libraries/RC_Channel/RC_Channel.cpp
// @Path: ../libraries/RC_Channel/RC_Channel.cpp,../libraries/RC_Channel/RC_Channel_aux.cpp
GGROUP(rc_6, "RC6_", RC_Channel_aux),
// @Group: RC7_
// @Path: ../libraries/RC_Channel/RC_Channel.cpp
// @Path: ../libraries/RC_Channel/RC_Channel.cpp,../libraries/RC_Channel/RC_Channel_aux.cpp
GGROUP(rc_7, "RC7_", RC_Channel_aux),
// @Group: RC8_
// @Path: ../libraries/RC_Channel/RC_Channel.cpp
// @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),
......@@ -188,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
......@@ -223,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
......@@ -280,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
......@@ -369,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.
......@@ -406,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),
......@@ -416,23 +477,25 @@ const AP_Param::Info var_info[] PROGMEM = {
// @Group: SR0_
// @Path: GCS_Mavlink.pde
GOBJECT(gcs0, "SR0_", GCS_MAVLINK),
GOBJECTN(gcs[0], gcs0, "SR0_", GCS_MAVLINK),
// @Group: SR1_
// @Path: GCS_Mavlink.pde
GOBJECTN(gcs[1], gcs1, "SR1_", GCS_MAVLINK),
// @Group: SR3_
#if MAVLINK_COMM_NUM_BUFFERS > 2
// @Group: SR2_
// @Path: GCS_Mavlink.pde
GOBJECT(gcs3, "SR3_", GCS_MAVLINK),
GOBJECTN(gcs[2], gcs2, "SR2_", GCS_MAVLINK),
#endif
// @Group: NAVL1_
// @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
......@@ -462,7 +525,26 @@ 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
};
......@@ -491,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
......@@ -75,13 +89,8 @@ static void calc_throttle(float target_speed)
return;
}
if (target_speed <= 0) {
// cope with zero requested speed
channel_throttle->servo_out = g.throttle_min.get();
return;
}
int throttle_target = g.throttle_cruise + throttle_nudge;
float throttle_base = (fabsf(target_speed) / g.speed_cruise) * g.throttle_cruise;
int throttle_target = throttle_base + throttle_nudge;
/*
reduce target speed in proportion to turning rate, up to the
......@@ -89,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;
}
......@@ -102,7 +118,7 @@ static void calc_throttle(float target_speed)
// reduce the target speed by the reduction factor
target_speed *= reduction;
groundspeed_error = target_speed - ground_speed;
groundspeed_error = fabsf(target_speed) - ground_speed;
throttle = throttle_target + (g.pidSpeedThrottle.get_pid(groundspeed_error * 100) / 100);
......@@ -110,7 +126,32 @@ static void calc_throttle(float target_speed)
// much faster response in turns
throttle *= reduction;
channel_throttle->servo_out = constrain_int16(throttle, g.throttle_min.get(), g.throttle_max.get());
if (in_reverse) {
channel_throttle->servo_out = constrain_int16(-throttle, -g.throttle_max, -g.throttle_min);
} 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;
}
}
/*****************************************
......@@ -138,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;
}
}
}
/*
......@@ -145,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;
......@@ -166,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();
......@@ -179,9 +223,15 @@ static void set_servos(void)
}
} else {
channel_steer->calc_pwm();
channel_throttle->servo_out = constrain_int16(channel_throttle->servo_out,
g.throttle_min.get(),
g.throttle_max.get());
if (in_reverse) {
channel_throttle->servo_out = constrain_int16(channel_throttle->servo_out,
-g.throttle_max,
-g.throttle_min);
} else {
channel_throttle->servo_out = constrain_int16(channel_throttle->servo_out,
g.throttle_min.get(),
g.throttle_max.get());
}
if ((failsafe.bits & FAILSAFE_EVENT_THROTTLE) && control_mode < AUTO) {
// suppress throttle if in failsafe
......@@ -193,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();
}
......@@ -220,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:
do_set_servo();
break;
case MAV_CMD_DO_SET_SERVO:
ServoRelayEvents.do_set_servo(cmd.content.servo.channel, cmd.content.servo.pwm);
break;
case MAV_CMD_DO_SET_RELAY:
do_set_relay();
break;
case MAV_CMD_DO_SET_RELAY:
ServoRelayEvents.do_set_relay(cmd.content.relay.num, cmd.content.relay.state);
break;
case MAV_CMD_DO_REPEAT_SERVO:
do_repeat_servo();
break;
case MAV_CMD_DO_REPEAT_SERVO:
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:
do_repeat_relay();
break;
case MAV_CMD_DO_REPEAT_RELAY:
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
......@@ -104,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();
......@@ -119,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;
}
......@@ -187,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;
next_WP = home;
}
static void do_takeoff()
static void do_nav_wp(const AP_Mission::Mission_Command& cmd)
{
set_next_WP(&next_nav_command);
}
static void do_nav_wp()
{
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;
}
......@@ -221,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;
}
......@@ -251,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
condition_value = cmd.content.delay.seconds * 1000; // convert seconds to milliseconds
}
static void do_change_alt()
static void do_within_distance(const AP_Mission::Mission_Command& cmd)
{
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
}
static void do_within_distance()
{
condition_value = next_nonnav_command.lat;
condition_value = cmd.content.distance.meters;
}
/********************************************************************************/
......@@ -283,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){
......@@ -305,115 +275,42 @@ static bool verify_within_distance()
// Do (Now) commands
/********************************************************************************/
static void do_jump()
static void do_change_speed(const AP_Mission::Mission_Command& cmd)
{
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()
{
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;
}
}
static void do_set_servo()
{
hal.rcout->enable_ch(next_nonnav_command.p1 - 1);
hal.rcout->write(next_nonnav_command.p1 - 1, next_nonnav_command.alt);
}
static void do_set_relay()
{
if (next_nonnav_command.p1 == 1) {
relay.on();
} else if (next_nonnav_command.p1 == 0) {
relay.off();
}else{
relay.toggle();
}
}
static void do_repeat_servo()
{
event_id = next_nonnav_command.p1 - 1;
if(next_nonnav_command.p1 >= CH_5 + 1 && next_nonnav_command.p1 <= CH_8 + 1) {
event_timer = 0;
event_delay = next_nonnav_command.lng * 500.0; // /2 (half cycle time) * 1000 (convert to milliseconds)
event_repeat = next_nonnav_command.lat * 2;
event_value = next_nonnav_command.alt;
event_undo_value = RC_Channel::rc_channel(next_nonnav_command.p1-1)->radio_trim;
update_events();
}
}
static void do_repeat_relay()
{
event_id = RELAY_TOGGLE;
event_timer = 0;
event_delay = next_nonnav_command.lat * 500.0; // /2 (half cycle time) * 1000 (convert to milliseconds)
event_repeat = next_nonnav_command.alt * 2;
update_events();
}
// do_take_picture - take a picture with the camera library
static void do_take_picture()
{
#if CAMERA == ENABLED
camera.trigger_pic();
if (g.log_bitmask & MASK_LOG_CAMERA) {
Log_Write_Camera();
gcs_send_message(MSG_CAMERA_FEEDBACK);
if (should_log(MASK_LOG_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
This diff is collapsed.
APMrover2/control_modes.pde
View file @ 62e7778b
This diff is collapsed.
APMrover2/defines.h
View file @ 62e7778b
This diff is collapsed.
APMrover2/events.pde
View file @ 62e7778b
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
static void update_events(void) // Used for MAV_CMD_DO_REPEAT_SERVO and MAV_CMD_DO_REPEAT_RELAY
static void update_events(void)
{
if(event_repeat == 0 || (millis() - event_timer) < event_delay)
return;
if (event_repeat > 0){
event_repeat --;
}
if(event_repeat != 0) { // event_repeat = -1 means repeat forever
event_timer = millis();
if (event_id >= CH_5 && event_id <= CH_8) {
if(event_repeat%2) {
hal.rcout->write(event_id, event_value); // send to Servos
} else {
hal.rcout->write(event_id, event_undo_value);
}
}
if (event_id == RELAY_TOGGLE) {
relay.toggle();
}
}
ServoRelayEvents.update_events();
}