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 (6842)
6,742 additional commits have been omitted to prevent performance issues.
Expand all Hide whitespace changes
Inline Side-by-side
Showing
with 2001 additions and 2975 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
*~
*.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/
Tools/ArdupilotMegaPlanner/bin/Release/logs/
ArduCopter/Debug/
ArduCopter/dataflash.bin
ArduCopter/eeprom.bin
ArduCopter/test/*
config.mk
serialsent.raw
CMakeFiles
CMakeCache.txt
cmake_install.cmake
build
dataflash.bin
eeprom.bin
.project
.settings/
.vagrant
/tmp/*
/Tools/ArdupilotMegaPlanner/3DRRadio/bin
/Tools/ArdupilotMegaPlanner/3DRRadio/obj
/Tools/ArdupilotMegaPlanner/APMPlannerXplanes/APMPlannerXplanes/Debug
......@@ -19,35 +36,47 @@ eeprom.bin
/Tools/ArdupilotMegaPlanner/bin/APM Planner.app
/Tools/ArdupilotMegaPlanner/bin/Debug
/Tools/ArdupilotMegaPlanner/bin/Release/gmapcache
/Tools/ArdupilotMegaPlanner/bin/Release/logs/
/Tools/ArdupilotMegaPlanner/CustomImages
/Tools/ArdupilotMegaPlanner/Msi/upload.bat
/Tools/ArdupilotMegaPlanner/obj
/Tools/ArdupilotMegaPlanner/resedit/bin
/Tools/ArdupilotMegaPlanner/resedit/obj
/Tools/ArdupilotMegaPlanner/Updater/obj
/Tools/ArdupilotMegaPlanner/Updater/bin
/Tools/ArdupilotMegaPlanner/Updater/obj
/Tools/ArdupilotMegaPlanner/wix/bin
/Tools/ArdupilotMegaPlanner/wix/obj
/Tools/autotest/ch7_mission.txt
tags
test.ArduCopter/*
/tmp/*
*.o
.*.swp
.*.swo
/Tools/autotest/aircraft/Rascal/reset.xml
/Tools/autotest/jsbsim/fgout.xml
/Tools/autotest/jsbsim/rascal_test.xml
ArduCopter/Debug/
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
module.mk
serialsent.raw
status.txt
*.exe
*.dll
*.obj
*.zip
*.pyc
Make.dep
.depend
.built
.context
*.lst
*.tlog
.DS_Store
tags
test.ArduCopter/*
Thumbs.db
.project deleted 100644 → 0
View file @ 855277fa
<?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 @ 855277fa
// -*- 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
// data stream rates. The code assumes that
// streamRateRawSensors is the first
AP_Int16 streamRateRawSensors;
AP_Int16 streamRateExtendedStatus;
AP_Int16 streamRateRCChannels;
AP_Int16 streamRateRawController;
AP_Int16 streamRatePosition;
AP_Int16 streamRateExtra1;
AP_Int16 streamRateExtra2;
AP_Int16 streamRateExtra3;
AP_Int16 streamRateParams;
// 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
......@@ -50,6 +50,8 @@ print_log_menu(void)
PLOG(CURRENT);
PLOG(SONAR);
PLOG(COMPASS);
PLOG(CAMERA);
PLOG(STEERING);
#undef PLOG
}
......@@ -141,6 +143,8 @@ select_logs(uint8_t argc, const Menu::arg *argv)
TARG(CURRENT);
TARG(SONAR);
TARG(COMPASS);
TARG(CAMERA);
TARG(STEERING);
#undef TARG
}
......@@ -161,95 +165,91 @@ 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
#if HIL_MODE != HIL_MODE_ATTITUDE
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));
}
#endif
struct PACKED log_Cmd {
// Write a mission command. Total length : 36 bytes
static void Log_Write_Cmd(const AP_Mission::Mission_Command &cmd)
{
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 {
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;
uint32_t time_ms;
float demanded_accel;
float achieved_accel;
};
// Write a command processing packet. Total length : 19 bytes
static void Log_Write_Cmd(uint8_t num, const struct Location *wp)
// Write a steering packet
static void Log_Write_Steering()
{
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
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));
}
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;
......@@ -258,29 +258,28 @@ struct PACKED log_Control_Tuning {
};
// Write a control tuning packet. Total length : 22 bytes
#if HIL_MODE != HIL_MODE_ATTITUDE
static void Log_Write_Control_Tuning()
{
Vector3f accel = ins.get_accel();
struct log_Control_Tuning pkt = {
LOG_PACKET_HEADER_INIT(LOG_CTUN_MSG),
steer_out : (int16_t)g.channel_steer.servo_out,
time_ms : millis(),
steer_out : (int16_t)channel_steer->servo_out,
roll : (int16_t)ahrs.roll_sensor,
pitch : (int16_t)ahrs.pitch_sensor,
throttle_out : (int16_t)g.channel_throttle.servo_out,
throttle_out : (int16_t)channel_throttle->servo_out,
accel_y : accel.y
};
DataFlash.WriteBlock(&pkt, sizeof(pkt));
}
#endif
struct PACKED log_Nav_Tuning {
LOG_PACKET_HEADER;
uint32_t time_ms;
uint16_t yaw;
float wp_distance;
uint16_t target_bearing_cd;
uint16_t nav_bearing_cd;
int16_t nav_gain_scalar;
int8_t throttle;
};
......@@ -289,18 +288,19 @@ 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)target_bearing,
nav_bearing_cd : (uint16_t)nav_bearing,
nav_gain_scalar : (int16_t)(nav_gain_scaler*1000),
throttle : (int8_t)(100 * g.channel_throttle.norm_output())
target_bearing_cd : (uint16_t)nav_controller->target_bearing_cd(),
nav_bearing_cd : (uint16_t)nav_controller->nav_bearing_cd(),
throttle : (int8_t)(100 * channel_throttle->norm_output())
};
DataFlash.WriteBlock(&pkt, sizeof(pkt));
}
struct PACKED log_Attitude {
LOG_PACKET_HEADER;
uint32_t time_ms;
int16_t roll;
int16_t pitch;
uint16_t yaw;
......@@ -312,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
};
......@@ -339,7 +350,8 @@ static void Log_Write_Mode()
struct PACKED log_Sonar {
LOG_PACKET_HEADER;
int16_t nav_steer;
uint32_t time_ms;
float lateral_accel;
uint16_t sonar1_distance;
uint16_t sonar2_distance;
uint16_t detected_count;
......@@ -350,7 +362,6 @@ struct PACKED log_Sonar {
};
// Write a sonar packet
#if HIL_MODE != HIL_MODE_ATTITUDE
static void Log_Write_Sonar()
{
uint16_t turn_time = 0;
......@@ -359,21 +370,22 @@ static void Log_Write_Sonar()
}
struct log_Sonar pkt = {
LOG_PACKET_HEADER_INIT(LOG_SONAR_MSG),
nav_steer : (int16_t)nav_steer_cd,
sonar1_distance : (uint16_t)sonar.distance_cm(),
sonar2_distance : (uint16_t)sonar2.distance_cm(),
time_ms : millis(),
lateral_accel : lateral_acceleration,
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,
ground_speed : (uint16_t)(ground_speed*100),
throttle : (int8_t)(100 * g.channel_throttle.norm_output())
throttle : (int8_t)(100 * channel_throttle->norm_output())
};
DataFlash.WriteBlock(&pkt, sizeof(pkt));
}
#endif
struct PACKED log_Current {
LOG_PACKET_HEADER;
uint32_t time_ms;
int16_t throttle_in;
int16_t battery_voltage;
int16_t current_amps;
......@@ -385,17 +397,22 @@ static void Log_Write_Current()
{
struct log_Current pkt = {
LOG_PACKET_HEADER_INIT(LOG_CURRENT_MSG),
throttle_in : g.channel_throttle.control_in,
battery_voltage : (int16_t)(battery_voltage1 * 100.0),
current_amps : (int16_t)(current_amps1 * 100.0),
board_voltage : board_voltage(),
current_total : current_total1
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 : (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;
......@@ -410,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,
......@@ -425,46 +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" },
"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", "HfHHhb", "Yaw,WpDist,TargBrg,NavBrg,NavGain,Thr" },
"NTUN", "IHfHHb", "TimeMS,Yaw,WpDist,TargBrg,NavBrg,Thr" },
{ LOG_SONAR_MSG, sizeof(log_Sonar),
"SONR", "hHHHbHCb", "NavStr,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);
}
......@@ -472,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() {}
......@@ -490,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,37 +28,63 @@ 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,
k_param_volt_div_ratio,
k_param_curr_amp_per_volt,
k_param_battery_monitoring = 140, // deprecated, can be deleted
k_param_volt_div_ratio, // deprecated, can be deleted
k_param_curr_amp_per_volt, // deprecated, can be deleted
k_param_input_voltage, // deprecated, can be deleted
k_param_pack_capacity,
k_param_pack_capacity, // deprecated, can be deleted
k_param_battery,
//
// 150: Navigation parameters
......@@ -71,13 +97,15 @@ public:
k_param_ch7_option,
k_param_auto_trigger_pin,
k_param_auto_kickstart,
k_param_turn_circle, // unused
k_param_turn_max_g,
//
// 160: Radio settings
//
k_param_channel_steer = 160,
k_param_rc_1 = 160,
k_param_rc_2,
k_param_channel_throttle,
k_param_rc_3,
k_param_rc_4,
k_param_rc_5,
k_param_rc_6,
......@@ -102,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
......@@ -119,25 +148,43 @@ public:
k_param_mode4,
k_param_mode5,
k_param_mode6,
k_param_learn_channel,
//
// 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,
//
// 230: camera control
//
k_param_camera,
k_param_camera_mount,
k_param_camera_mount2,
//
// 240: PID Controllers
k_param_pidNavSteer = 230,
k_param_pidServoSteer,
k_param_pidServoSteer, // unused
k_param_pidSpeedThrottle,
// high RC channels
k_param_rc_9 = 235,
k_param_rc_10,
k_param_rc_11,
k_param_rc_12,
// other objects
k_param_sitl = 240,
k_param_ahrs,
k_param_ins,
k_param_compass,
k_param_rcmap,
k_param_L1_controller,
k_param_steerController,
k_param_barometer,
// 254,255: reserved
};
......@@ -147,52 +194,66 @@ public:
// Misc
//
AP_Int16 log_bitmask;
AP_Int32 log_bitmask;
AP_Int16 num_resets;
AP_Int8 reset_switch_chan;
AP_Int8 initial_mode;
// IO pins
AP_Int8 rssi_pin;
AP_Int8 battery_volt_pin;
AP_Int8 battery_curr_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;
// sensor parameters
AP_Int8 compass_enabled;
// battery controls
AP_Int8 battery_monitoring; // 0=disabled, 3=voltage only, 4=voltage and current
AP_Float volt_div_ratio;
AP_Float curr_amp_per_volt;
AP_Int16 pack_capacity; // Battery pack capacity less reserve
AP_Int8 compass_enabled;
// navigation parameters
//
AP_Float crosstrack_gain;
AP_Int16 crosstrack_entry_angle;
AP_Float speed_cruise;
AP_Int8 speed_turn_gain;
AP_Float speed_turn_dist;
AP_Int8 ch7_option;
AP_Int8 auto_trigger_pin;
AP_Float auto_kickstart;
AP_Float turn_max_g;
AP_Int16 pivot_turn_angle;
// RC channels
RC_Channel channel_steer;
RC_Channel rc_1;
RC_Channel_aux rc_2;
RC_Channel channel_throttle;
RC_Channel rc_3;
RC_Channel_aux rc_4;
RC_Channel_aux rc_5;
RC_Channel_aux rc_6;
RC_Channel_aux rc_7;
RC_Channel_aux rc_8;
#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 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
RC_Channel_aux rc_10;
RC_Channel_aux rc_11;
#endif
#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
//
......@@ -226,34 +287,41 @@ public:
AP_Int8 mode4;
AP_Int8 mode5;
AP_Int8 mode6;
AP_Int8 learn_channel;
// Waypoints
//
AP_Int8 command_total;
AP_Int8 command_index;
AP_Float waypoint_radius;
// PID controllers
//
PID pidNavSteer;
PID pidServoSteer;
PID pidSpeedThrottle;
Parameters() :
// RC channels
channel_steer(CH_1),
rc_1(CH_1),
rc_2(CH_2),
channel_throttle(CH_3),
rc_3(CH_3),
rc_4(CH_4),
rc_5(CH_5),
rc_6(CH_6),
rc_7(CH_7),
rc_8(CH_8),
#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 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
rc_10 (CH_10),
rc_11 (CH_11),
#endif
#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
//-----------------------------------------------------------------------------------
pidNavSteer (0.7, 0.1, 0.2, 2000),
pidServoSteer (0.5, 0.1, 0.2, 2000),
pidSpeedThrottle (0.7, 0.2, 0.2, 4000)
{}
};
......
APMrover2/Parameters.pde
View file @ 62e7778b
This diff is collapsed.
APMrover2/Steering.pde
View file @ 62e7778b
......@@ -6,14 +6,14 @@
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(g.channel_throttle.radio_max - g.channel_throttle.radio_min);
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
if (temp < 1) {
temp = 1;
}
g.channel_throttle.radio_out = constrain_int16(g.channel_throttle.radio_out, last_throttle - temp, last_throttle + temp);
channel_throttle->radio_out = constrain_int16(channel_throttle->radio_out, last_throttle - temp, last_throttle + temp);
}
}
......@@ -27,6 +27,13 @@ static bool auto_check_trigger(void)
return true;
}
// check for user pressing the auto trigger to off
if (auto_triggered && g.auto_trigger_pin != -1 && check_digital_pin(g.auto_trigger_pin) == 1) {
gcs_send_text_P(SEVERITY_LOW, PSTR("AUTO triggered off"));
auto_triggered = false;
return false;
}
// if already triggered, then return true, so you don't
// need to hold the switch down
if (auto_triggered) {
......@@ -39,21 +46,10 @@ static bool auto_check_trigger(void)
return true;
}
if (g.auto_trigger_pin != -1) {
int8_t pin = hal.gpio->analogPinToDigitalPin(g.auto_trigger_pin);
if (pin != -1) {
// ensure we are in input mode
hal.gpio->pinMode(pin, GPIO_INPUT);
// enable pullup
hal.gpio->write(pin, 1);
if (hal.gpio->read(pin) == 0) {
gcs_send_text_P(SEVERITY_LOW, PSTR("Triggered AUTO with pin"));
auto_triggered = true;
return true;
}
}
if (g.auto_trigger_pin != -1 && check_digital_pin(g.auto_trigger_pin) == 0) {
gcs_send_text_P(SEVERITY_LOW, PSTR("Triggered AUTO with pin"));
auto_triggered = true;
return true;
}
if (g.auto_kickstart != 0.0f) {
......@@ -68,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,29 +85,31 @@ static bool auto_check_trigger(void)
static void calc_throttle(float target_speed)
{
if (!auto_check_trigger()) {
g.channel_throttle.servo_out = g.throttle_min.get();
channel_throttle->servo_out = g.throttle_min.get();
return;
}
if (target_speed <= 0) {
// cope with zero requested speed
g.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
SPEED_TURN_GAIN percentage.
*/
float steer_rate = fabsf((nav_steer_cd/nav_gain_scaler) / (float)SERVO_MAX);
steer_rate = constrain(steer_rate, 0.0, 1.0);
float reduction = 1.0 - steer_rate*(100 - g.speed_turn_gain)*0.01;
float steer_rate = fabsf(lateral_acceleration / (g.turn_max_g*GRAVITY_MSS));
steer_rate = constrain_float(steer_rate, 0.0, 1.0);
// 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;
}
......@@ -106,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);
......@@ -114,34 +126,81 @@ static void calc_throttle(float target_speed)
// much faster response in turns
throttle *= reduction;
g.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;
}
}
/*****************************************
* Calculate desired turn angles (in medium freq loop)
*****************************************/
static void calc_nav_steer()
static void calc_lateral_acceleration()
{
// Adjust gain based on ground speed
if (ground_speed < 0.01) {
nav_gain_scaler = 1.4f;
} else {
nav_gain_scaler = g.speed_cruise / ground_speed;
switch (control_mode) {
case AUTO:
nav_controller->update_waypoint(prev_WP, next_WP);
break;
case RTL:
case GUIDED:
case STEERING:
nav_controller->update_waypoint(current_loc, next_WP);
break;
default:
return;
}
nav_gain_scaler = constrain(nav_gain_scaler, 0.2f, 1.4f);
// Calculate the required turn of the wheels rover
// ----------------------------------------
// Calculate the required turn of the wheels
// negative error = left turn
// positive error = right turn
nav_steer_cd = g.pidNavSteer.get_pid_4500(bearing_error_cd, nav_gain_scaler);
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;
}
}
}
/*
calculate steering angle given lateral_acceleration
*/
static void calc_nav_steer()
{
// add in obstacle avoidance
lateral_acceleration += (obstacle.turn_angle/45.0f) * g.turn_max_g;
// avoid obstacles, if any
nav_steer_cd += obstacle.turn_angle*100;
// constrain to max G force
lateral_acceleration = constrain_float(lateral_acceleration, -g.turn_max_g*GRAVITY_MSS, g.turn_max_g*GRAVITY_MSS);
g.channel_steer.servo_out = nav_steer_cd;
channel_steer->servo_out = steerController.get_steering_out_lat_accel(lateral_acceleration);
}
/*****************************************
......@@ -149,88 +208,73 @@ static void calc_nav_steer()
*****************************************/
static void set_servos(void)
{
int16_t last_throttle = g.channel_throttle.radio_out;
static int16_t last_throttle;
if ((control_mode == MANUAL || control_mode == LEARNING) &&
(g.skid_steer_out == g.skid_steer_in)) {
// 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) {
// do a direct pass through of radio values
g.channel_steer.radio_out = hal.rcin->read(CH_STEER);
g.channel_throttle.radio_out = hal.rcin->read(CH_THROTTLE);
channel_steer->radio_out = channel_steer->read();
channel_throttle->radio_out = channel_throttle->read();
if (failsafe.bits & FAILSAFE_EVENT_THROTTLE) {
// suppress throttle if in failsafe and manual
g.channel_throttle.radio_out = g.channel_throttle.radio_trim;
channel_throttle->radio_out = channel_throttle->radio_trim;
}
} else {
g.channel_steer.calc_pwm();
g.channel_throttle.servo_out = constrain_int16(g.channel_throttle.servo_out,
g.throttle_min.get(),
g.throttle_max.get());
channel_steer->calc_pwm();
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
g.channel_throttle.servo_out = 0;
channel_throttle->servo_out = 0;
}
// convert 0 to 100% into PWM
g.channel_throttle.calc_pwm();
channel_throttle->calc_pwm();
// 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 = g.channel_steer.norm_output();
float throttle_scaled = g.channel_throttle.norm_output();
float motor1 = throttle_scaled + 0.5*steering_scaled;
float motor2 = throttle_scaled - 0.5*steering_scaled;
g.channel_steer.servo_out = 4500*motor1;
g.channel_throttle.servo_out = 100*motor2;
g.channel_steer.calc_pwm();
g.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();
}
#if HIL_MODE == HIL_MODE_DISABLED || HIL_SERVOS
// send values to the PWM timers for output
// ----------------------------------------
hal.rcout->write(CH_1, g.channel_steer.radio_out); // send to Servos
hal.rcout->write(CH_3, g.channel_throttle.radio_out); // send to Servos
// 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);
channel_steer->output();
channel_throttle->output();
RC_Channel_aux::output_ch_all();
#endif
}
static bool demoing_servos;
static void demo_servos(uint8_t i) {
while(i > 0) {
gcs_send_text_P(SEVERITY_LOW,PSTR("Demo Servos!"));
demoing_servos = true;
#if HIL_MODE == HIL_MODE_DISABLED || HIL_SERVOS
hal.rcout->write(1, 1400);
mavlink_delay(400);
hal.rcout->write(1, 1600);
mavlink_delay(200);
hal.rcout->write(1, 1500);
#endif
demoing_servos = false;
mavlink_delay(400);
i--;
}
}
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
......@@ -121,14 +33,8 @@ static void set_next_WP(const struct Location *wp)
}
// this is handy for the groundstation
wp_totalDistance = get_distance(&current_loc, &next_WP);
wp_totalDistance = get_distance(current_loc, next_WP);
wp_distance = wp_totalDistance;
target_bearing = get_bearing_cd(&current_loc, &next_WP);
nav_bearing = target_bearing;
// set a new crosstrack bearing
// ----------------------------
reset_crosstrack();
}
static void set_guided_WP(void)
......@@ -142,13 +48,8 @@ static void set_guided_WP(void)
next_WP = guided_WP;
// this is handy for the groundstation
wp_totalDistance = get_distance(&current_loc, &next_WP);
wp_totalDistance = get_distance(current_loc, next_WP);
wp_distance = wp_totalDistance;
target_bearing = get_bearing_cd(&current_loc, &next_WP);
// set a new crosstrack bearing
// ----------------------------
reset_crosstrack();
}
// run this at setup on the ground
......@@ -162,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, 0);
home.alt = g_gps->altitude;
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
// -------------
......@@ -185,10 +80,7 @@ void init_home()
static void restart_nav()
{
g.pidNavSteer.reset_I();
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
This diff is collapsed.
APMrover2/commands_process.pde
View file @ 62e7778b
This diff is collapsed.
APMrover2/compat.h
View file @ 62e7778b
This diff is collapsed.
APMrover2/compat.pde
View file @ 62e7778b
This diff is collapsed.
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.