Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Rover: major update, fixing lots of bugs 

This removes a lot of the ArduPlane specific cruft left over from the
initial Rover import from ArduPlane, plus fixes a bunch of serious
bugs. For example, the rover was unable to use either compass or gps
for heading - it used just gyros!

There is still a lot of cruft left, but this at least gives a bit
better basis for future work
This commit is contained in:
Andrew Tridgell 2012-11-17 17:45:20 +11:00
parent 531e62d3b5
commit abc0a1cd38
20 changed files with 82 additions and 980 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
APMrover2/APM_Config.h
View File

@ -8,9 +8,9 @@
//#define SERIAL3_BAUD 38400
//#define GCS_PROTOCOL GCS_PROTOCOL_NONE
#define CONFIG_APM_HARDWARE APM_HARDWARE_APM2
//#define CONFIG_APM_HARDWARE APM_HARDWARE_APM2
#define LITE ENABLED // if LITE is ENABLED, you may use an APM1280 or APM2560 CPU only (IMU less) with a GPS MT3329
#define LITE DISABLED // if LITE is ENABLED, you may use an APM1280 or APM2560 CPU only (IMU less) with a GPS MT3329
// if LITE is DISABLED, this is for a full APM v1 (Oilpan + GPS MT3329 + Magnetometer HMC5883L) or APM v2
#define CLI_ENABLED ENABLED
@ -18,10 +18,16 @@
#define CLOSED_LOOP_NAV ENABLED
#define AUTO_WP_RADIUS DISABLED
#define TRACE ENABLED
#define TRACE DISABLED
// sonar is currently broken - it causes an APM1 to crash
// after a few seconds
#define CONFIG_SONAR DISABLED
//#define LOGGING_ENABLED DISABLED
//#include "APM_Config_HILmode.h" // for test in HIL mode with AeroSIM Rc 3.83
#include "APM_Config_Rover.h" // to be used with the real Traxxas model Monster Jam Grinder
//#include "APM_Config_Rover.h" // to be used with the real Traxxas model Monster Jam Grinder
// Radio setup:
// APM INPUT (Rec = receiver)

4
APMrover2/APM_Config_HILmode.h
View File

@ -44,8 +44,6 @@
#define FLIGHT_MODE_5 MANUAL // pos 4
#define FLIGHT_MODE_6 MANUAL // pos 5 ---
#define ENABLE_AIR_START DISABLED
#define MANUAL_LEVEL DISABLED
#define CLOSED_LOOP_NAV ENABLED // set to ENABLED if closed loop navigation else set to DISABLED (Return To Lauch)
@ -131,8 +129,6 @@
#define AUTO_TRIM ENABLED
#define THROTTLE_FAILSAFE DISABLED
//#define ENABLE_AIR_START 0
//////////////////////////////////////////////////////////////////////////////
// Autopilot control limits
//

4
APMrover2/APM_Config_Rover.h
View File

@ -43,8 +43,6 @@
#define FLIGHT_MODE_5 MANUAL // pos 4
#define FLIGHT_MODE_6 MANUAL // pos 5 ---
#define ENABLE_AIR_START DISABLED
#define MANUAL_LEVEL DISABLED
#define TURN_GAIN 5
@ -139,8 +137,6 @@ This feature works only if the ROV_AWPR_NAV is set to 0
#define AUTO_TRIM ENABLED
#define THROTTLE_FAILSAFE DISABLED
//#define ENABLE_AIR_START 0
//////////////////////////////////////////////////////////////////////////////
// Autopilot control limits
//

129
APMrover2/APMrover2.pde
View File

@ -89,7 +89,7 @@ version 2.1 of the License, or (at your option) any later version.
#include <AP_ADC.h> // ArduPilot Mega Analog to Digital Converter Library
#include <AP_AnalogSource.h>// ArduPilot Mega polymorphic analog getter
#include <AP_PeriodicProcess.h> // ArduPilot Mega TimerProcess
#include <AP_Baro.h> // ArduPilot barometer library
#include <AP_Baro.h>
#include <AP_Compass.h> // ArduPilot Mega Magnetometer Library
#include <AP_Math.h> // ArduPilot Mega Vector/Matrix math Library
#include <AP_InertialSensor.h> // Inertial Sensor (uncalibated IMU) Library
@ -207,20 +207,11 @@ static AP_ADC_ADS7844 adc;
#endif
#ifdef DESKTOP_BUILD
AP_Baro_BMP085_HIL barometer;
AP_Compass_HIL compass;
AP_Baro_BMP085_HIL barometer;
AP_Compass_HIL compass;
#include <SITL.h>
SITL sitl;
#else
#if CONFIG_BARO == AP_BARO_BMP085
# if CONFIG_APM_HARDWARE == APM_HARDWARE_APM2
static AP_Baro_BMP085 barometer(true);
# else
static AP_Baro_BMP085 barometer(false);
# endif
#elif CONFIG_BARO == AP_BARO_MS5611
static AP_Baro_MS5611 barometer;
#endif
static AP_Compass_HMC5843 compass;
#endif
@ -261,7 +252,6 @@ AP_AHRS_DCM ahrs(&ins, g_gps);
#elif HIL_MODE == HIL_MODE_SENSORS
// sensor emulators
AP_ADC_HIL adc;
AP_Baro_BMP085_HIL barometer;
AP_Compass_HIL compass;
AP_GPS_HIL g_gps_driver(NULL);
AP_InertialSensor_Oilpan ins( &adc );
@ -272,7 +262,6 @@ AP_ADC_HIL adc;
AP_AHRS_HIL ahrs(&ins, g_gps);
AP_GPS_HIL g_gps_driver(NULL);
AP_Compass_HIL compass; // never used
AP_Baro_BMP085_HIL barometer;
#else
#error Unrecognised HIL_MODE setting.
#endif // HIL MODE
@ -477,26 +466,7 @@ static byte non_nav_command_index;
static byte nav_command_ID = NO_COMMAND;
static byte non_nav_command_ID = NO_COMMAND;
////////////////////////////////////////////////////////////////////////////////
// Airspeed
////////////////////////////////////////////////////////////////////////////////
// The current airspeed estimate/measurement in centimeters per second
static int airspeed;
// The calculated airspeed to use in FBW-B. Also used in higher modes for insuring min ground speed is met.
// Also used for flap deployment criteria. Centimeters per second.static long target_airspeed;
static long target_airspeed;
// The difference between current and desired airspeed. Used in the pitch controller. Centimeters per second.
static float airspeed_error;
static float groundspeed_error;
// The calculated total energy error (kinetic (altitude) plus potential (airspeed)).
// Used by the throttle controller
static long energy_error;
// kinetic portion of energy error (m^2/s^2)
static long airspeed_energy_error;
// An amount that the airspeed should be increased in auto modes based on the user positioning the
// throttle stick in the top half of the range. Centimeters per second.
static int airspeed_nudge;
// Similar to airspeed_nudge, but used when no airspeed sensor.
// 0-(throttle_max - throttle_cruise) : throttle nudge in Auto mode using top 1/2 of throttle stick travel
static int throttle_nudge = 0;
// The distance as reported by Sonar in cm – Values are 20 to 700 generally.
@ -549,45 +519,8 @@ static float current_total1;
//static float current_amps2; // Current (Amperes) draw from battery 2
//static float current_total2; // Totalized current (Amp-hours) from battery 2
////////////////////////////////////////////////////////////////////////////////
// Airspeed Sensors
////////////////////////////////////////////////////////////////////////////////
// Raw differential pressure measurement (filtered). ADC units
static float airspeed_raw;
// Raw differential pressure less the zero pressure offset. ADC units
static float airspeed_pressure;
// The pressure at home location - calibrated at arming
static int32_t ground_pressure;
// The ground temperature at home location - calibrated at arming
static int16_t ground_temperature;
////////////////////////////////////////////////////////////////////////////////
// Altitude Sensor variables
////////////////////////////////////////////////////////////////////////////////
// Raw absolute pressure measurement (filtered). ADC units
static unsigned long abs_pressure;
// The altitude as reported by Baro in cm – Values can be quite high
static int32_t baro_alt;
////////////////////////////////////////////////////////////////////////////////
// flight mode specific
////////////////////////////////////////////////////////////////////////////////
// Flag for using gps ground course instead of IMU yaw. Set false when takeoff command in process.
static bool takeoff_complete = true;
// Flag to indicate if we have landed.
//Set land_complete if we are within 2 seconds distance or within 3 meters altitude of touchdown
static bool land_complete;
// Altitude threshold to complete a takeoff command in autonomous modes. Centimeters
static long takeoff_altitude;
// Pitch to hold during landing command in the no airspeed sensor case. Hundredths of a degree
static int landing_pitch;
// Minimum pitch to hold during takeoff command execution. Hundredths of a degree
static int takeoff_pitch;
static bool final = false;
// JLN Update
unsigned long timesw = 0;
static long ground_course = 0; // deg * 100 dir of plane
static bool speed_boost = false;
////////////////////////////////////////////////////////////////////////////////
@ -615,8 +548,6 @@ static long nav_roll;
static long nav_pitch;
// Calculated radius for the wp turn based on ground speed and max turn angle
static long wp_radius;
static long toff_yaw; // deg * 100 : yaw angle for takeoff
static long altitude_estimate = 0; // for smoothing GPS output
////////////////////////////////////////////////////////////////////////////////
// Waypoint distances
@ -626,8 +557,6 @@ static long wp_distance;
// Distance between previous and next waypoint. Meters
static long wp_totalDistance;
static long max_dist_set; // used for HEADALT (LEO)
////////////////////////////////////////////////////////////////////////////////
// repeating event control
////////////////////////////////////////////////////////////////////////////////
@ -678,14 +607,6 @@ static struct Location next_nav_command;
// The location structure information from the Non-Nav command being processed
static struct Location next_nonnav_command;
////////////////////////////////////////////////////////////////////////////////
// Altitude / Climb rate control
////////////////////////////////////////////////////////////////////////////////
// The current desired altitude. Altitude is linearly ramped between waypoints. Centimeters
static long target_altitude;
// Altitude difference between previous and current waypoint. Centimeters
static long offset_altitude;
////////////////////////////////////////////////////////////////////////////////
// IMU variables
////////////////////////////////////////////////////////////////////////////////
@ -1025,10 +946,10 @@ static void slow_loop()
#if TRACE == ENABLED
// Serial.printf_P(PSTR("NAV->gnd_crs=%3.0f, nav_brg=%3.0f, tgt_brg=%3.0f, brg_err=%3.0f, nav_rll=%3.1f rsvo=%3.1f\n"),
// (float)ground_course/100, (float)nav_bearing/100, (float)target_bearing/100, (float)bearing_error/100, (float)nav_roll/100, (float)g.channel_roll.servo_out/100);
// ahrs.yaw_sensor*0.01, (float)nav_bearing/100, (float)target_bearing/100, (float)bearing_error/100, (float)nav_roll/100, (float)g.channel_roll.servo_out/100);
// Serial.printf_P(PSTR("WPL->g.command_total=%d, g.command_index=%d, nav_command_index=%d\n"),
// g.command_total, g.command_index, nav_command_index);
Serial.printf_P(PSTR("NAV->gnd_crs=%3.0f, sonar_dist = %d obstacle = %d\n"), (float)ground_course/100, (int)sonar_dist, obstacle);
Serial.printf_P(PSTR("NAV->gnd_crs=%3.0f, sonar_dist = %d obstacle = %d\n"), ahrs.yaw_sensor*0.01, (int)sonar_dist, obstacle);
#endif
break;
}
@ -1045,8 +966,7 @@ static void one_second_loop()
}
static void update_GPS(void)
{ static uint16_t hdg;
{
g_gps->update();
update_GPS_light();
@ -1067,22 +987,11 @@ static void update_GPS(void)
ground_start_count = 5;
} else {
if(ENABLE_AIR_START == 1 && (ground_start_avg / 5) < SPEEDFILT){
startup_ground();
#if LITE == DISABLED
if (g.log_bitmask & MASK_LOG_CMD)
Log_Write_Startup(TYPE_GROUNDSTART_MSG);
#endif
init_home();
} else if (ENABLE_AIR_START == 0) {
init_home();
}
//#if LITE == DISABLED
init_home();
if (g.compass_enabled) {
// Set compass declination automatically
compass.set_initial_location(g_gps->latitude, g_gps->longitude);
}
//#endif
ground_start_count = 0;
}
}
@ -1090,31 +999,17 @@ static void update_GPS(void)
current_loc.lng = g_gps->longitude; // Lon * 10**7
current_loc.lat = g_gps->latitude; // Lat * 10**7
current_loc.alt = max((g_gps->altitude - home.alt),0);
ground_speed = g_gps->ground_speed;
#if LITE == DISABLED
if (g.compass_enabled) {
hdg=(ahrs.yaw_sensor / 100) % 360;
ground_course = hdg * 100;
ground_course = ahrs.yaw_sensor;
} else {
#endif
ground_course = ToDeg(ahrs.yaw) * 100;
#if LITE == DISABLED
}
#endif
// see if we've breached the geo-fence
geofence_check(false);
current_loc.alt = max((g_gps->altitude - home.alt),0);
ground_speed = g_gps->ground_speed;
}
}
static void update_current_flight_mode(void)
{ int AOAstart;
{
if(control_mode == AUTO){
switch(nav_command_ID){
case MAV_CMD_NAV_TAKEOFF:
case MAV_CMD_NAV_LAND:
break;
default:
hold_course = -1;

58
APMrover2/Attitude.pde
View File

@ -15,43 +15,32 @@ static void learning()
}
static void crash_checker()
{
if(ahrs.pitch_sensor < -4500){
crash_timer = 255;
}
if(crash_timer > 0)
crash_timer--;
}
static void calc_throttle()
{ int rov_speed;
int throttle_target = g.throttle_cruise + throttle_nudge + 50;
target_airspeed = g.airspeed_cruise;
rov_speed = g.airspeed_cruise;
if(speed_boost)
rov_speed = g.booster * target_airspeed;
else
rov_speed = target_airspeed;
if (speed_boost)
rov_speed *= g.booster;
groundspeed_error = rov_speed - (float)ground_speed;
groundspeed_error = rov_speed - (float)ground_speed;
int throttle_req = (throttle_target + g.pidTeThrottle.get_pid(groundspeed_error)) * 10;
int throttle_req = (throttle_target + g.pidTeThrottle.get_pid(groundspeed_error)) * 10;
if(g.throttle_slewrate > 0)
{ if (throttle_req > throttle_last)
throttle = throttle + g.throttle_slewrate;
else if (throttle_req < throttle_last) {
throttle = throttle - g.throttle_slewrate;
}
throttle = constrain(throttle, 500, throttle_req);
throttle_last = throttle;
} else {
throttle = throttle_req;
}
g.channel_throttle.servo_out = constrain(((float)throttle / 10.0f), 0, g.throttle_max.get());
if(g.throttle_slewrate > 0) {
if (throttle_req > throttle_last)
throttle = throttle + g.throttle_slewrate;
else if (throttle_req < throttle_last) {
throttle = throttle - g.throttle_slewrate;
}
throttle = constrain(throttle, 500, throttle_req);
throttle_last = throttle;
} else {
throttle = throttle_req;
}
g.channel_throttle.servo_out = constrain(((float)throttle / 10.0f), 0, g.throttle_max.get());
}
/*****************************************
@ -95,19 +84,6 @@ static void reset_I(void)
*****************************************/
static void set_servos(void)
{
int flapSpeedSource = 0;
// vectorize the rc channels
RC_Channel_aux* rc_array[NUM_CHANNELS];
rc_array[CH_1] = NULL;
rc_array[CH_2] = NULL;
rc_array[CH_3] = NULL;
rc_array[CH_4] = NULL;
rc_array[CH_5] = &g.rc_5;
rc_array[CH_6] = &g.rc_6;
rc_array[CH_7] = &g.rc_7;
rc_array[CH_8] = &g.rc_8;
if((control_mode == MANUAL) || (control_mode == LEARNING)){
// do a direct pass through of radio values
g.channel_roll.radio_out = g.channel_roll.radio_in;

112
APMrover2/GCS_Mavlink.pde
View File

@ -98,22 +98,13 @@ static NOINLINE void send_attitude(mavlink_channel_t chan)
chan,
micros(),
ahrs.roll,
ahrs.pitch - radians(g.pitch_trim*0.01),
ToRad(ground_course/100.0),
//ahrs.yaw,
ahrs.pitch,
ahrs.yaw,
omega.x,
omega.y,
omega.z);
}
#if GEOFENCE_ENABLED == ENABLED
static NOINLINE void send_fence_status(mavlink_channel_t chan)
{
geofence_send_status(chan);
}
#endif
static NOINLINE void send_extended_status1(mavlink_channel_t chan, uint16_t packet_drops)
{
uint32_t control_sensors_present = 0;
@ -356,9 +347,9 @@ static void NOINLINE send_vfr_hud(mavlink_channel_t chan)
{
mavlink_msg_vfr_hud_send(
chan,
(float)airspeed / 100.0,
(float)g_gps->ground_speed / 100.0,
ground_course,
(float)g_gps->ground_speed / 100.0,
(ahrs.yaw_sensor / 100) % 360,
(uint16_t)(100 * (g.channel_throttle.norm_output() / 2.0 + 0.5)), // scale -1,1 to 0-100
current_loc.alt / 100.0,
0);
@ -384,17 +375,6 @@ static void NOINLINE send_raw_imu1(mavlink_channel_t chan)
compass.mag_z);
}
static void NOINLINE send_raw_imu2(mavlink_channel_t chan)
{
int32_t pressure = 0;
mavlink_msg_scaled_pressure_send(
chan,
micros(),
pressure/100.0,
(pressure - g.ground_pressure)/100.0,
0);
}
static void NOINLINE send_raw_imu3(mavlink_channel_t chan)
{
Vector3f mag_offsets = compass.get_offsets();
@ -427,26 +407,10 @@ static void NOINLINE send_ahrs(mavlink_channel_t chan)
#endif // HIL_MODE != HIL_MODE_ATTITUDE
#ifdef DESKTOP_BUILD
void mavlink_simstate_send(uint8_t chan,
float roll,
float pitch,
float yaw,
float xAcc,
float yAcc,
float zAcc,
float p,
float q,
float r)
{
mavlink_msg_simstate_send((mavlink_channel_t)chan,
roll, pitch, yaw, xAcc, yAcc, zAcc, p, q, r);
}
// report simulator state
static void NOINLINE send_simstate(mavlink_channel_t chan)
{
extern void sitl_simstate_send(uint8_t chan);
sitl_simstate_send((uint8_t)chan);
sitl.simstate_send(chan);
}
#endif
@ -586,11 +550,6 @@ static bool mavlink_try_send_message(mavlink_channel_t chan, enum ap_message id,
send_raw_imu1(chan);
break;
case MSG_RAW_IMU2:
CHECK_PAYLOAD_SIZE(SCALED_PRESSURE);
send_raw_imu2(chan);
break;
case MSG_RAW_IMU3:
CHECK_PAYLOAD_SIZE(SENSOR_OFFSETS);
send_raw_imu3(chan);
@ -630,13 +589,6 @@ static bool mavlink_try_send_message(mavlink_channel_t chan, enum ap_message id,
send_statustext(chan);
break;
#if GEOFENCE_ENABLED == ENABLED
case MSG_FENCE_STATUS:
CHECK_PAYLOAD_SIZE(FENCE_STATUS);
send_fence_status(chan);
break;
#endif
case MSG_AHRS:
#if HIL_MODE != HIL_MODE_ATTITUDE
CHECK_PAYLOAD_SIZE(AHRS);
@ -883,7 +835,6 @@ GCS_MAVLINK::data_stream_send(void)
if (stream_trigger(STREAM_RAW_SENSORS)) {
send_message(MSG_RAW_IMU1);
send_message(MSG_RAW_IMU2);
send_message(MSG_RAW_IMU3);
}
@ -893,7 +844,6 @@ GCS_MAVLINK::data_stream_send(void)
send_message(MSG_CURRENT_WAYPOINT);
send_message(MSG_GPS_RAW); // TODO - remove this message after location message is working
send_message(MSG_NAV_CONTROLLER_OUTPUT);
send_message(MSG_FENCE_STATUS);
if (last_gps_satellites != g_gps->num_sats) {
// this message is mostly a huge waste of bandwidth,
@ -963,7 +913,6 @@ GCS_MAVLINK::send_text(gcs_severity severity, const prog_char_t *str)
void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
{
struct Location tell_command = {}; // command for telemetry
static uint8_t mav_nav=255; // For setting mode (some require receipt of 2 messages...)
switch (msg->msgid) {
@ -1074,7 +1023,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
packet.param2 == 1 ||
packet.param3 == 1) {
#if LITE == DISABLED
startup_IMU_ground(true);
startup_INS_ground(true);
#endif
}
if (packet.param4 == 1) {
@ -1422,7 +1371,6 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
case MAV_CMD_NAV_WAYPOINT:
case MAV_CMD_NAV_LOITER_UNLIM:
case MAV_CMD_NAV_RETURN_TO_LAUNCH:
case MAV_CMD_NAV_LAND:
break;
case MAV_CMD_NAV_LOITER_TURNS:
@ -1537,42 +1485,6 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
break;
}
#if GEOFENCE_ENABLED == ENABLED
// receive a fence point from GCS and store in EEPROM
case MAVLINK_MSG_ID_FENCE_POINT: {
mavlink_fence_point_t packet;
mavlink_msg_fence_point_decode(msg, &packet);
if (mavlink_check_target(packet.target_system, packet.target_component))
break;
if (g.fence_action != FENCE_ACTION_NONE) {
send_text(SEVERITY_LOW,PSTR("fencing must be disabled"));
} else if (packet.count != g.fence_total) {
send_text(SEVERITY_LOW,PSTR("bad fence point"));
} else {
Vector2l point;
point.x = packet.lat*1.0e7;
point.y = packet.lng*1.0e7;
set_fence_point_with_index(point, packet.idx);
}
break;
}
// send a fence point to GCS
case MAVLINK_MSG_ID_FENCE_FETCH_POINT: {
mavlink_fence_fetch_point_t packet;
mavlink_msg_fence_fetch_point_decode(msg, &packet);
if (mavlink_check_target(packet.target_system, packet.target_component))
break;
if (packet.idx >= g.fence_total) {
send_text(SEVERITY_LOW,PSTR("bad fence point"));
} else {
Vector2l point = get_fence_point_with_index(packet.idx);
mavlink_msg_fence_point_send(chan, 0, 0, packet.idx, g.fence_total,
point.x*1.0e-7, point.y*1.0e-7);
}
break;
}
#endif // GEOFENCE_ENABLED
case MAVLINK_MSG_ID_PARAM_SET:
{
@ -1793,18 +1705,6 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
break;
}
case MAVLINK_MSG_ID_RAW_PRESSURE:
{
// decode
mavlink_raw_pressure_t packet;
mavlink_msg_raw_pressure_decode(msg, &packet);
// set pressure hil sensor
// TODO: check scaling
float temp = 70;
barometer.setHIL(temp,packet.press_diff1 + 101325);
break;
}
#endif // HIL_MODE
#if MOUNT == ENABLED

22
APMrover2/Log.pde
View File

@ -50,10 +50,10 @@ MENU2(log_menu, "Log", log_menu_commands, print_log_menu);
static bool
print_log_menu(void)
{
int log_start;
int log_end;
int temp;
int last_log_num = DataFlash.find_last_log();
int16_t log_start;
int16_t log_end;
int16_t temp;
int16_t last_log_num = DataFlash.find_last_log();
uint16_t num_logs = DataFlash.get_num_logs();
@ -105,9 +105,9 @@ print_log_menu(void)
static int8_t
dump_log(uint8_t argc, const Menu::arg *argv)
{
int dump_log;
int dump_log_start;
int dump_log_end;
int16_t dump_log;
int16_t dump_log_start;
int16_t dump_log_end;
byte last_log_num;
// check that the requested log number can be read
@ -324,7 +324,7 @@ static void Log_Write_Nav_Tuning()
DataFlash.WriteInt((uint16_t)target_bearing);
DataFlash.WriteInt((uint16_t)nav_bearing);
DataFlash.WriteInt(altitude_error);
DataFlash.WriteInt((int)airspeed);
DataFlash.WriteInt(0);
DataFlash.WriteInt((int)(nav_gain_scaler*1000));
DataFlash.WriteByte(END_BYTE);
}
@ -358,7 +358,7 @@ static void Log_Write_GPS( int32_t log_Time, int32_t log_Lattitude, int32_t log_
DataFlash.WriteLong(log_Ground_Course);
DataFlash.WriteInt(0);
DataFlash.WriteInt(0);
DataFlash.WriteInt((int)airspeed);
DataFlash.WriteInt(0);
DataFlash.WriteByte(END_BYTE);
}
@ -525,13 +525,13 @@ static void Log_Read_GPS()
{
int32_t l[7];
byte b[2];
int16_t i,j,k,m;
int16_t j,k,m;
l[0] = DataFlash.ReadLong();
b[0] = DataFlash.ReadByte();
b[1] = DataFlash.ReadByte();
l[1] = DataFlash.ReadLong();
l[2] = DataFlash.ReadLong();
i = DataFlash.ReadInt();
DataFlash.ReadInt();
l[3] = DataFlash.ReadLong();
l[4] = DataFlash.ReadLong();
l[5] = DataFlash.ReadLong();

17
APMrover2/Parameters.h
View File

@ -17,7 +17,7 @@ public:
// The increment will prevent old parameters from being used incorrectly
// by newer code.
//
static const uint16_t k_format_version = 13;
static const uint16_t k_format_version = 14;
// The parameter software_type is set up solely for ground station use
// and identifies the software type (eg ArduPilotMega versus ArduCopterMega)
@ -37,7 +37,6 @@ public:
k_param_auto_trim,
k_param_switch_enable,
k_param_log_bitmask,
k_param_pitch_trim,
k_param_mix_mode,
k_param_reverse_elevons,
k_param_reverse_ch1_elevon,
@ -90,7 +89,7 @@ public:
k_param_sonar_type,
#endif
#endif
k_param_airspeed_enabled,
k_param_ahrs, // AHRS group
//
// 150: Navigation parameters
@ -101,8 +100,6 @@ public:
k_param_pitch_limit_max,
k_param_pitch_limit_min,
k_param_airspeed_cruise,
k_param_RTL_altitude,
k_param_inverted_flight_ch,
k_param_min_gndspeed,
k_param_ch7_option,
//
@ -263,13 +260,6 @@ public:
AP_Int8 command_index;
AP_Int8 waypoint_radius;
AP_Int8 loiter_radius;
#if GEOFENCE_ENABLED == ENABLED
AP_Int8 fence_action;
AP_Int8 fence_total;
AP_Int8 fence_channel;
AP_Int16 fence_minalt; // meters
AP_Int16 fence_maxalt; // meters
#endif
// Fly-by-wire
//
@ -324,8 +314,6 @@ public:
AP_Int16 min_gndspeed;
AP_Int8 ch7_option;
AP_Int16 pitch_trim;
AP_Int16 RTL_altitude;
AP_Int16 ground_temperature;
AP_Int32 ground_pressure;
AP_Int8 compass_enabled;
@ -344,7 +332,6 @@ public:
// 3 = HRLV
#endif
#endif
AP_Int8 airspeed_enabled;
AP_Int8 flap_1_percent;
AP_Int8 flap_1_speed;
AP_Int8 flap_2_percent;

24
APMrover2/Parameters.pde
View File

@ -30,25 +30,12 @@ const AP_Param::Info var_info[] PROGMEM = {
GSCALAR(crosstrack_entry_angle, "XTRK_ANGLE_CD", XTRACK_ENTRY_ANGLE_CENTIDEGREE),
GSCALAR(altitude_mix, "ALT_MIX", ALTITUDE_MIX),
GSCALAR(airspeed_ratio, "ARSPD_RATIO", AIRSPEED_RATIO),
GSCALAR(airspeed_offset, "ARSPD_OFFSET", 0),
GSCALAR(command_total, "CMD_TOTAL", 0),
GSCALAR(command_index, "CMD_INDEX", 0),
GSCALAR(waypoint_radius, "WP_RADIUS", WP_RADIUS_DEFAULT),
GSCALAR(loiter_radius, "WP_LOITER_RAD", LOITER_RADIUS_DEFAULT),
#if GEOFENCE_ENABLED == ENABLED
GSCALAR(fence_action, "FENCE_ACTION", 0),
GSCALAR(fence_total, "FENCE_TOTAL", 0),
GSCALAR(fence_channel, "FENCE_CHANNEL", 0),
GSCALAR(fence_minalt, "FENCE_MINALT", 0),
GSCALAR(fence_maxalt, "FENCE_MAXALT", 0),
#endif
GSCALAR(flybywire_airspeed_min, "ARSPD_FBW_MIN", AIRSPEED_FBW_MIN),
GSCALAR(flybywire_airspeed_max, "ARSPD_FBW_MAX", AIRSPEED_FBW_MAX),
GSCALAR(throttle_min, "THR_MIN", THROTTLE_MIN),
GSCALAR(throttle_max, "THR_MAX", THROTTLE_MAX),
GSCALAR(throttle_slewrate, "THR_SLEWRATE", THROTTLE_SLEW_LIMIT),
@ -86,12 +73,6 @@ const AP_Param::Info var_info[] PROGMEM = {
GSCALAR(min_gndspeed, "MIN_GNDSPD_CM", MIN_GNDSPEED_CM),
GSCALAR(ch7_option, "CH7_OPT", CH7_OPTION),
GSCALAR(pitch_trim, "TRIM_PITCH_CD", 0),
GSCALAR(RTL_altitude, "ALT_HOLD_RTL", ALT_HOLD_HOME_CM),
GSCALAR(FBWB_min_altitude, "ALT_HOLD_FBWCM", ALT_HOLD_FBW_CM),
GSCALAR(ground_temperature, "GND_TEMP", 0),
GSCALAR(ground_pressure, "GND_ABS_PRESS", 0),
GSCALAR(compass_enabled, "MAG_ENABLE", MAGNETOMETER),
GSCALAR(flap_1_percent, "FLAP_1_PERCNT", FLAP_1_PERCENT),
GSCALAR(flap_1_speed, "FLAP_1_SPEED", FLAP_2_SPEED),
@ -116,7 +97,6 @@ const AP_Param::Info var_info[] PROGMEM = {
GSCALAR(sonar_type, "SONAR_TYPE", AP_RANGEFINDER_MAXSONARXL),
#endif
#endif
GSCALAR(airspeed_enabled, "ARSPD_ENABLE", AIRSPEED_SENSOR),
// ************************************************************
// APMrover parameters - JLN update
@ -157,6 +137,10 @@ const AP_Param::Info var_info[] PROGMEM = {
GOBJECT(ins, "INS_", AP_InertialSensor),
#endif
// @Group: AHRS_
// @Path: ../libraries/AP_AHRS/AP_AHRS.cpp
GOBJECT(ahrs, "AHRS_", AP_AHRS),
AP_VAREND
};

71
APMrover2/commands.pde
View File

@ -2,8 +2,6 @@
/* Functions in this file:
void init_commands()
void update_auto()
void reload_commands()
struct Location get_cmd_with_index(int i)
void set_cmd_with_index(struct Location temp, int i)
void increment_cmd_index()
@ -24,34 +22,6 @@ static void init_commands()
next_nav_command.id = CMD_BLANK;
}
static void update_auto()
{
if (g.command_index >= g.command_total) {
handle_no_commands();
if(g.command_total == 0) {
next_WP.lat = home.lat + 1000; // so we don't have bad calcs
next_WP.lng = home.lng + 1000; // so we don't have bad calcs
}
} else {
if(g.command_index != 0) {
g.command_index = nav_command_index;
nav_command_index--;
}
nav_command_ID = NO_COMMAND;
non_nav_command_ID = NO_COMMAND;
next_nav_command.id = CMD_BLANK;
process_next_command();
}
}
// Reload all the wp
static void reload_commands()
{
init_commands();
g.command_index.load(); // XXX can we assume it's been loaded already by ::load_all?
decrement_cmd_index();
}
// Getters
// -------
static struct Location get_cmd_with_index(int i)
@ -98,7 +68,7 @@ static struct Location get_cmd_with_index(int i)
static void set_cmd_with_index(struct Location temp, int i)
{
i = constrain(i, 0, g.command_total.get());
uint32_t mem = WP_START_BYTE + (i * WP_SIZE);
intptr_t mem = WP_START_BYTE + (i * WP_SIZE);
// Set altitude options bitmask
// XXX What is this trying to do?
@ -126,29 +96,6 @@ static void set_cmd_with_index(struct Location temp, int i)
eeprom_write_dword((uint32_t *) mem, temp.lng);
}
static void increment_cmd_index()
{
if (g.command_index <= g.command_total) {
g.command_index.set_and_save(g.command_index + 1);
}
}
static void decrement_cmd_index()
{
if (g.command_index > 0) {
g.command_index.set_and_save(g.command_index - 1);
}
}
static long read_alt_to_hold()
{
if(g.RTL_altitude < 0)
return current_loc.alt;
else
return g.RTL_altitude + home.alt;
}
/*
This function stores waypoint commands
It looks to see what the next command type is and finds the last command.
@ -163,15 +110,6 @@ static void set_next_WP(struct Location *wp)
// ---------------------
next_WP = *wp;
// used to control FBW and limit the rate of climb
// -----------------------------------------------
target_altitude = current_loc.alt;
if(prev_WP.id != MAV_CMD_NAV_TAKEOFF && prev_WP.alt != home.alt && (next_WP.id == MAV_CMD_NAV_WAYPOINT || next_WP.id == MAV_CMD_NAV_LAND))
offset_altitude = next_WP.alt - prev_WP.alt;
else
offset_altitude = 0;
// zero out our loiter vals to watch for missed waypoints
loiter_delta = 0;
loiter_sum = 0;
@ -206,11 +144,6 @@ static void set_guided_WP(void)
// ---------------------
next_WP = guided_WP;
// used to control FBW and limit the rate of climb
// -----------------------------------------------
target_altitude = current_loc.alt;
offset_altitude = next_WP.alt - prev_WP.alt;
// this is used to offset the shrinking longitude as we go towards the poles
float rads = (abs(next_WP.lat)/t7) * 0.0174532925;
scaleLongDown = cos(rads);
@ -279,8 +212,6 @@ void init_home()
// Load home for a default guided_WP
// -------------
guided_WP = home;
guided_WP.alt += g.RTL_altitude;
}
static void restart_nav()

26
APMrover2/commands_logic.pde
View File

@ -21,10 +21,6 @@ handle_process_nav_cmd()
do_nav_wp();
break;
case MAV_CMD_NAV_LAND: // LAND to Waypoint
do_land();
break;
case MAV_CMD_NAV_LOITER_UNLIM: // Loiter indefinitely
do_loiter_unlimited();
break;
@ -127,7 +123,6 @@ static void handle_no_commands()
{
gcs_send_text_fmt(PSTR("Returning to Home"));
next_nav_command = home;
next_nav_command.alt = read_alt_to_hold();
next_nav_command.id = MAV_CMD_NAV_LOITER_UNLIM;
nav_command_ID = MAV_CMD_NAV_LOITER_UNLIM;
non_nav_command_ID = WAIT_COMMAND;
@ -146,10 +141,6 @@ static bool verify_nav_command() // Returns true if command complete
return verify_takeoff();
break;
case MAV_CMD_NAV_LAND:
return verify_land();
break;
case MAV_CMD_NAV_WAYPOINT:
return verify_nav_wp();
break;
@ -217,11 +208,6 @@ static void do_RTL(void)
control_mode = RTL;
crash_timer = 0;
next_WP = home;
// Altitude to hold over home
// Set by configuration tool
// -------------------------
next_WP.alt = read_alt_to_hold();
}
static void do_takeoff()
@ -234,11 +220,6 @@ static void do_nav_wp()
set_next_WP(&next_nav_command);
}
static void do_land()
{
set_next_WP(&next_nav_command);
}
static void do_loiter_unlimited()
{
set_next_WP(&next_nav_command);
@ -264,10 +245,6 @@ static bool verify_takeoff()
{ return true;
}
static bool verify_land()
{
}
static void calc_turn_radius(void) // JLN update - adjut automaticaly the wp_radius Vs the speed and the turn angle
{
wp_radius = ground_speed * 150 / g.roll_limit.get();
@ -361,9 +338,7 @@ static void do_change_alt()
condition_rate = abs((int)next_nonnav_command.lat);
condition_value = next_nonnav_command.alt;
if(condition_value < current_loc.alt) condition_rate = -condition_rate;
target_altitude = current_loc.alt + (condition_rate / 10); // Divide by ten for 10Hz update
next_WP.alt = condition_value; // For future nav calculations
offset_altitude = 0; // For future nav calculations
}
static void do_within_distance()
@ -390,7 +365,6 @@ static bool verify_change_alt()
condition_value = 0;
return true;
}
target_altitude += condition_rate / 10;
return false;
}

93
APMrover2/config.h
View File

@ -115,18 +115,6 @@
# define CURRENT_PIN_1 2
#endif
//////////////////////////////////////////////////////////////////////////////
// AIRSPEED_SENSOR
// AIRSPEED_RATIO
//
#ifndef AIRSPEED_SENSOR
# define AIRSPEED_SENSOR DISABLED
#endif
#ifndef AIRSPEED_RATIO
# define AIRSPEED_RATIO 1.9936 // Note - this varies from the value in ArduPilot due to the difference in ADC resolution
#endif
//////////////////////////////////////////////////////////////////////////////
// IMU Selection
//
@ -145,40 +133,6 @@
# endif
#endif
//////////////////////////////////////////////////////////////////////////////
// Barometer
//
#ifndef CONFIG_BARO
# define CONFIG_BARO AP_BARO_BMP085
#endif
//////////////////////////////////////////////////////////////////////////////
// Pitot
//
#ifndef PITOT_ENABLED
# define PITOT_ENABLED DISABLED
#endif
#ifndef CONFIG_PITOT_SOURCE
# define CONFIG_PITOT_SOURCE PITOT_SOURCE_ADC
#endif
#if CONFIG_PITOT_SOURCE == PITOT_SOURCE_ADC
# ifndef CONFIG_PITOT_SOURCE_ADC_CHANNEL
# define CONFIG_PITOT_SOURCE_ADC_CHANNEL 7
# endif
#elif CONFIG_PITOT_SOURCE == PITOT_SOURCE_ANALOG_PIN
# ifndef CONFIG_PITOT_SOURCE_ANALOG_PIN
# define CONFIG_PITOT_SOURCE_ANALOG_PIN 0
# endif
#else
# warning Invalid value for CONFIG_PITOT_SOURCE, disabling airspeed
# undef PITOT_ENABLED
# define PITOT_ENABLED DISABLED
#endif
#ifndef SONAR_TYPE
# define SONAR_TYPE MAX_SONAR_LV // MAX_SONAR_XL,
#endif
@ -471,13 +425,6 @@
# define GROUND_START_DELAY 0
#endif
//////////////////////////////////////////////////////////////////////////////
// ENABLE_AIR_START
//
#ifndef ENABLE_AIR_START
# define ENABLE_AIR_START DISABLED
#endif
//////////////////////////////////////////////////////////////////////////////
// ENABLE REVERSE_SWITCH
//
@ -570,23 +517,6 @@
#endif
#define MIN_GNDSPEED_CM MIN_GNDSPEED*100
//////////////////////////////////////////////////////////////////////////////
// FLY_BY_WIRE_B airspeed control
//
#ifndef AIRSPEED_FBW_MIN
# define AIRSPEED_FBW_MIN 6
#endif
#ifndef AIRSPEED_FBW_MAX
# define AIRSPEED_FBW_MAX 22
#endif
#ifndef ALT_HOLD_FBW
# define ALT_HOLD_FBW 0
#endif
#define ALT_HOLD_FBW_CM ALT_HOLD_FBW*100
/* The following parmaeters have no corresponding control implementation
#ifndef THROTTLE_ALT_P
# define THROTTLE_ALT_P 0.32
@ -884,22 +814,6 @@
# define CLI_SLIDER_ENABLED ENABLED
#endif
// use this to disable gen-fencing
#ifndef GEOFENCE_ENABLED
# define GEOFENCE_ENABLED ENABLED
#endif
// pwm value on FENCE_CHANNEL to use to enable fenced mode
#ifndef FENCE_ENABLE_PWM
# define FENCE_ENABLE_PWM 1750
#endif
// a digital pin to set high when the geo-fence triggers. Defaults
// to -1, which means don't activate a pin
#ifndef FENCE_TRIGGERED_PIN
# define FENCE_TRIGGERED_PIN -1
#endif
// if RESET_SWITCH_CH is not zero, then this is the PWM value on
// that channel where we reset the control mode to the current switch
// position (to for example return to switched mode after failsafe or
@ -908,3 +822,10 @@
# define RESET_SWITCH_CHAN_PWM 1750
#endif
#ifndef BOOSTER
# define BOOSTER 2 // booster factor x1 = 1 or x2 = 2
#endif
#ifndef SONAR_ENABLED
# define SONAR_ENABLED DISABLED
#endif

11
APMrover2/defines.h
View File

@ -147,7 +147,6 @@ enum ap_message {
MSG_RADIO_OUT,
MSG_RADIO_IN,
MSG_RAW_IMU1,
MSG_RAW_IMU2,
MSG_RAW_IMU3,
MSG_GPS_STATUS,
MSG_GPS_RAW,
@ -155,7 +154,6 @@ enum ap_message {
MSG_NEXT_WAYPOINT,
MSG_NEXT_PARAM,
MSG_STATUSTEXT,
MSG_FENCE_STATUS,
MSG_AHRS,
MSG_SIMSTATE,
MSG_HWSTATUS,
@ -237,15 +235,10 @@ enum gcs_severity {
// EEPROM addresses
#define EEPROM_MAX_ADDR 4096
// parameters get the first 1KiB of EEPROM, remainder is for waypoints
#define WP_START_BYTE 0x400 // where in memory home WP is stored + all other WP
#define WP_START_BYTE 0x500 // where in memory home WP is stored + all other WP
#define WP_SIZE 15
// fence points are stored at the end of the EEPROM
#define MAX_FENCEPOINTS 20
#define FENCE_WP_SIZE sizeof(Vector2l)
#define FENCE_START_BYTE (EEPROM_MAX_ADDR-(MAX_FENCEPOINTS*FENCE_WP_SIZE))
#define MAX_WAYPOINTS ((FENCE_START_BYTE - WP_START_BYTE) / WP_SIZE) - 1 // - 1 to be safe
#define MAX_WAYPOINTS ((EEPROM_MAX_ADDR - WP_START_BYTE) / WP_SIZE) - 1 // - 1 to be safe
#define ONBOARD_PARAM_NAME_LENGTH 15

325
APMrover2/geofence.pde
View File

@ -1,325 +0,0 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
/*
geo-fencing support
Andrew Tridgell, December 2011
*/
#if GEOFENCE_ENABLED == ENABLED
/*
The state of geo-fencing. This structure is dynamically allocated
the first time it is used. This means we only pay for the pointer
and not the structure on systems where geo-fencing is not being
used.
We store a copy of the boundary in memory as we need to access it
very quickly at runtime
*/
static struct geofence_state {
uint8_t num_points;
bool boundary_uptodate;
bool fence_triggered;
uint16_t breach_count;
uint8_t breach_type;
uint32_t breach_time;
byte old_switch_position;
/* point 0 is the return point */
Vector2l boundary[MAX_FENCEPOINTS];
} *geofence_state;
/*
fence boundaries fetch/store
*/
static Vector2l get_fence_point_with_index(unsigned i)
{
uint32_t mem;
Vector2l ret;
if (i > (unsigned)g.fence_total) {
return Vector2l(0,0);
}
// read fence point
mem = FENCE_START_BYTE + (i * FENCE_WP_SIZE);
ret.x = eeprom_read_dword((uint32_t *)mem);
mem += sizeof(uint32_t);
ret.y = eeprom_read_dword((uint32_t *)mem);
return ret;
}
// save a fence point
static void set_fence_point_with_index(Vector2l &point, unsigned i)
{
uint32_t mem;
if (i >= (unsigned)g.fence_total.get()) {
// not allowed
return;
}
mem = FENCE_START_BYTE + (i * FENCE_WP_SIZE);
eeprom_write_dword((uint32_t *)mem, point.x);
mem += sizeof(uint32_t);
eeprom_write_dword((uint32_t *)mem, point.y);
if (geofence_state != NULL) {
geofence_state->boundary_uptodate = false;
}
}
/*
allocate and fill the geofence state structure
*/
static void geofence_load(void)
{
uint8_t i;
if (geofence_state == NULL) {
if (memcheck_available_memory() < 512 + sizeof(struct geofence_state)) {
// too risky to enable as we could run out of stack
goto failed;
}
geofence_state = (struct geofence_state *)calloc(1, sizeof(struct geofence_state));
if (geofence_state == NULL) {
// not much we can do here except disable it
goto failed;
}
}
for (i=0; i<g.fence_total; i++) {
geofence_state->boundary[i] = get_fence_point_with_index(i);
}
geofence_state->num_points = i;
if (!Polygon_complete(&geofence_state->boundary[1], geofence_state->num_points-1)) {
// first point and last point must be the same
goto failed;
}
if (Polygon_outside(geofence_state->boundary[0], &geofence_state->boundary[1], geofence_state->num_points-1)) {
// return point needs to be inside the fence
goto failed;
}
geofence_state->boundary_uptodate = true;
geofence_state->fence_triggered = false;
gcs_send_text_P(SEVERITY_LOW,PSTR("geo-fence loaded"));
gcs_send_message(MSG_FENCE_STATUS);
return;
failed:
g.fence_action.set(FENCE_ACTION_NONE);
gcs_send_text_P(SEVERITY_HIGH,PSTR("geo-fence setup error"));
}
/*
return true if geo-fencing is enabled
*/
static bool geofence_enabled(void)
{
if (g.fence_action == FENCE_ACTION_NONE ||
g.fence_channel == 0 ||
APM_RC.InputCh(g.fence_channel-1) < FENCE_ENABLE_PWM) {
// geo-fencing is disabled
if (geofence_state != NULL) {
// re-arm for when the channel trigger is switched on
geofence_state->fence_triggered = false;
}
return false;
}
if (!g_gps->fix) {
// we can't do much without a GPS fix
return false;
}
return true;
}
/*
return true if we have breached the geo-fence minimum altiude
*/
static bool geofence_check_minalt(void)
{
if (g.fence_maxalt <= g.fence_minalt) {
return false;
}
if (g.fence_minalt == 0) {
return false;
}
return (current_loc.alt < (g.fence_minalt*100) + home.alt);
}
/*
return true if we have breached the geo-fence maximum altiude
*/
static bool geofence_check_maxalt(void)
{
if (g.fence_maxalt <= g.fence_minalt) {
return false;
}
if (g.fence_maxalt == 0) {
return false;
}
return (current_loc.alt > (g.fence_maxalt*100) + home.alt);
}
/*
check if we have breached the geo-fence
*/
static void geofence_check(bool altitude_check_only)
{
if (!geofence_enabled()) {
// switch back to the chosen control mode if still in
// GUIDED to the return point
if (geofence_state != NULL &&
g.fence_action == FENCE_ACTION_GUIDED &&
g.fence_channel != 0 &&
control_mode == GUIDED &&
g.fence_total >= 5 &&
geofence_state->boundary_uptodate &&
geofence_state->old_switch_position == oldSwitchPosition &&
guided_WP.lat == geofence_state->boundary[0].x &&
guided_WP.lng == geofence_state->boundary[0].y) {
geofence_state->old_switch_position = 0;
reset_control_switch();
}
return;
}
/* allocate the geo-fence state if need be */
if (geofence_state == NULL || !geofence_state->boundary_uptodate) {
geofence_load();
if (!geofence_enabled()) {
// may have been disabled by load
return;
}
}
bool outside = false;
uint8_t breach_type = FENCE_BREACH_NONE;
if (geofence_check_minalt()) {
outside = true;
breach_type = FENCE_BREACH_MINALT;
} else if (geofence_check_maxalt()) {
outside = true;
breach_type = FENCE_BREACH_MAXALT;
} else if (!altitude_check_only) {
Vector2l location;
location.x = current_loc.lat;
location.y = current_loc.lng;
outside = Polygon_outside(location, &geofence_state->boundary[1], geofence_state->num_points-1);
if (outside) {
breach_type = FENCE_BREACH_BOUNDARY;
}
}
if (!outside) {
if (geofence_state->fence_triggered && !altitude_check_only) {
// we have moved back inside the fence
geofence_state->fence_triggered = false;
gcs_send_text_P(SEVERITY_LOW,PSTR("geo-fence OK"));
#if FENCE_TRIGGERED_PIN > 0
digitalWrite(FENCE_TRIGGERED_PIN, LOW);
#endif
gcs_send_message(MSG_FENCE_STATUS);
}
// we're inside, all is good with the world
return;
}
// we are outside the fence
if (geofence_state->fence_triggered && control_mode == GUIDED) {
// we have already triggered, don't trigger again until the
// user disables/re-enables using the fence channel switch
return;
}
// we are outside, and have not previously triggered.
geofence_state->fence_triggered = true;
geofence_state->breach_count++;
geofence_state->breach_time = millis();
geofence_state->breach_type = breach_type;
#if FENCE_TRIGGERED_PIN > 0
digitalWrite(FENCE_TRIGGERED_PIN, HIGH);
#endif
gcs_send_text_P(SEVERITY_LOW,PSTR("geo-fence triggered"));
gcs_send_message(MSG_FENCE_STATUS);
// see what action the user wants
switch (g.fence_action) {
case FENCE_ACTION_GUIDED:
// fly to the return point, with an altitude half way between
// min and max
if (g.fence_minalt >= g.fence_maxalt) {
// invalid min/max, use RTL_altitude
guided_WP.alt = home.alt + (g.RTL_altitude * 100);
} else {
guided_WP.alt = home.alt + 100*(g.fence_minalt + g.fence_maxalt)/2;
}
guided_WP.id = 0;
guided_WP.p1 = 0;
guided_WP.options = 0;
guided_WP.lat = geofence_state->boundary[0].x;
guided_WP.lng = geofence_state->boundary[0].y;
geofence_state->old_switch_position = oldSwitchPosition;
if (control_mode == MANUAL && g.auto_trim) {
// make sure we don't auto trim the surfaces on this change
control_mode = LEARNING;
}
set_mode(GUIDED);
break;
}
}
/*
return true if geofencing allows stick mixing. When we have
triggered failsafe and are in GUIDED mode then stick mixing is
disabled. Otherwise the aircraft may not be able to recover from
a breach of the geo-fence
*/
static bool geofence_stickmixing(void) {
if (geofence_enabled() &&
geofence_state != NULL &&
geofence_state->fence_triggered &&
control_mode == GUIDED) {
// don't mix in user input
return false;
}
// normal mixing rules
return true;
}
/*
*/
static void geofence_send_status(mavlink_channel_t chan)
{
if (geofence_enabled() && geofence_state != NULL) {
mavlink_msg_fence_status_send(chan,
(int8_t)geofence_state->fence_triggered,
geofence_state->breach_count,
geofence_state->breach_type,
geofence_state->breach_time);
}
}
#else // GEOFENCE_ENABLED
static void geofence_check(bool altitude_check_only) { }
static bool geofence_stickmixing(void) { return true; }
static bool geofence_enabled(void) { return false; }
#endif // GEOFENCE_ENABLED

2
APMrover2/navigation.pde
View File

@ -79,7 +79,7 @@ static void calc_gndspeed_undershoot()
static void calc_bearing_error()
{
bearing_error = nav_bearing - ground_course;
bearing_error = nav_bearing - ahrs.yaw_sensor;
bearing_error = wrap_180(bearing_error);
}

11
APMrover2/radio.pde
View File

@ -70,9 +70,6 @@ static void init_rc_out()
static void read_radio()
{
static uint16_t aileron1;
static uint16_t aileron2;
ch1_temp = APM_RC.InputCh(CH_ROLL);
ch2_temp = APM_RC.InputCh(CH_PITCH);
@ -101,14 +98,8 @@ static void read_radio()
g.channel_throttle.servo_out = g.channel_throttle.control_in;
if (g.channel_throttle.servo_out > 50) {
if(g.airspeed_enabled == true) {
airspeed_nudge = (g.flybywire_airspeed_max * 100 - g.airspeed_cruise) * ((g.channel_throttle.norm_input()-0.5) / 0.5);
airspeed_nudge = NUDGE_OFFSET + airspeed_nudge;
} else {
throttle_nudge = (g.throttle_max - g.throttle_cruise) * ((g.channel_throttle.norm_input()-0.5) / 0.5);
}
throttle_nudge = (g.throttle_max - g.throttle_cruise) * ((g.channel_throttle.norm_input()-0.5) / 0.5);
} else {
airspeed_nudge = NUDGE_OFFSET;
throttle_nudge = 0;
}

77
APMrover2/sensors.pde
View File

@ -18,83 +18,6 @@ static void init_sonar(void)
void ReadSCP1000(void) {}
static void init_barometer(void)
{
int flashcount = 0;
long ground_pressure = 0;
int ground_temperature;
while (ground_pressure == 0 || !barometer.healthy) {
barometer.read(); // Get initial data from absolute pressure sensor
ground_pressure = baro_filter.apply(barometer.get_pressure());
//ground_pressure = barometer.get_pressure();
ground_temperature = barometer.get_temperature();
mavlink_delay(20);
//Serial.printf("barometer.Press %ld\n", barometer.get_pressure());
}
for(int i = 0; i < 30; i++){ // We take some readings...
#if HIL_MODE == HIL_MODE_SENSORS
gcs_update(); // look for inbound hil packets
#endif
do {
barometer.read(); // Get pressure sensor
} while (!barometer.healthy);
ground_pressure = baro_filter.apply(barometer.get_pressure());
//ground_pressure = (ground_pressure * 9l + barometer.get_pressure()) / 10l;
ground_temperature = (ground_temperature * 9 + barometer.get_temperature()) / 10;
mavlink_delay(20);
if(flashcount == 5) {
digitalWrite(C_LED_PIN, LED_OFF);
digitalWrite(A_LED_PIN, LED_ON);
digitalWrite(B_LED_PIN, LED_OFF);
}
if(flashcount >= 10) {
flashcount = 0;
digitalWrite(C_LED_PIN, LED_ON);
digitalWrite(A_LED_PIN, LED_OFF);
digitalWrite(B_LED_PIN, LED_ON);
}
flashcount++;
}
g.ground_pressure.set_and_save(ground_pressure);
g.ground_temperature.set_and_save(ground_temperature / 10.0f);
abs_pressure = ground_pressure;
Serial.printf_P(PSTR("abs_pressure %ld\n"), abs_pressure);
gcs_send_text_P(SEVERITY_MEDIUM, PSTR("barometer calibration complete."));
}
static int32_t read_barometer(void)
{
float x, scaling, temp;
barometer.read(); // Get new data from absolute pressure sensor
float abs_pressure = baro_filter.apply(barometer.get_pressure());
//abs_pressure = (abs_pressure + barometer.get_pressure()) >> 1; // Small filtering
//abs_pressure = ((float)abs_pressure * .7) + ((float)barometer.get_pressure() * .3); // large filtering
scaling = (float)g.ground_pressure / (float)abs_pressure;
temp = ((float)g.ground_temperature) + 273.15f;
x = log(scaling) * temp * 29271.267f;
return (x / 10);
}
// in M/S * 100
static void read_airspeed(void)
{
}
static void zero_airspeed(void)
{
}
#endif // HIL_MODE != HIL_MODE_ATTITUDE
#endif

2
APMrover2/setup.pde
View File

@ -578,7 +578,7 @@ static void zero_eeprom(void)
{
byte b = 0;
Serial.printf_P(PSTR("\nErasing EEPROM\n"));
for (int i = 0; i < EEPROM_MAX_ADDR; i++) {
for (intptr_t i = 0; i < EEPROM_MAX_ADDR; i++) {
eeprom_write_byte((uint8_t *) i, b);
}
Serial.printf_P(PSTR("done\n"));

34
APMrover2/system.pde
View File

@ -179,8 +179,6 @@ static void init_ardupilot()
#endif
#if LITE == DISABLED
barometer.init(&timer_scheduler);
if (g.compass_enabled==true) {
compass.set_orientation(MAG_ORIENTATION); // set compass's orientation on aircraft
if (!compass.init()|| !compass.read()) {
@ -259,11 +257,6 @@ static void init_ardupilot()
DDRL |= B00000100; // Set Port L, pin 2 to output for the relay
#endif
#if FENCE_TRIGGERED_PIN > 0
pinMode(FENCE_TRIGGERED_PIN, OUTPUT);
digitalWrite(FENCE_TRIGGERED_PIN, LOW);
#endif
/*
setup the 'main loop is dead' check. Note that this relies on
the RC library being initialised.
@ -302,7 +295,8 @@ static void init_ardupilot()
if (g.log_bitmask & MASK_LOG_CMD)
Log_Write_Startup(TYPE_GROUNDSTART_MSG);
#endif
set_mode(MANUAL);
set_mode(MANUAL);
// set the correct flight mode
// ---------------------------
@ -335,13 +329,10 @@ static void startup_ground(void)
startup_INS_ground(false);
#endif
// read the radio to set trims
// ---------------------------
trim_radio(); // This was commented out as a HACK. Why? I don't find a problem.
// Save the settings for in-air restart
// ------------------------------------
//save_EEPROM_groundstart();
trim_radio();
// initialize commands
// -------------------
@ -351,11 +342,11 @@ static void startup_ground(void)
// -----------------------
demo_servos(3);
gcs_send_text_P(SEVERITY_LOW,PSTR("\n\n Ready to FLY."));
gcs_send_text_P(SEVERITY_LOW,PSTR("\n\n Ready to drive."));
}
static void set_mode(byte mode)
{ struct Location temp;
{
if(control_mode == mode){
// don't switch modes if we are already in the correct mode.
@ -469,19 +460,6 @@ static void startup_INS_ground(bool force_accel_level)
ahrs.set_fly_forward(true);
ahrs.reset();
// read Baro pressure at ground
//-----------------------------
init_barometer();
if (g.airspeed_enabled == true) {
// initialize airspeed sensor
// --------------------------
zero_airspeed();
gcs_send_text_P(SEVERITY_LOW,PSTR("<startup_ground> zero airspeed calibrated"));
} else {
gcs_send_text_P(SEVERITY_LOW,PSTR("<startup_ground> NO airspeed"));
}
#endif // HIL_MODE_ATTITUDE
digitalWrite(B_LED_PIN, LED_ON); // Set LED B high to indicate INS ready

26
APMrover2/test.pde
View File

@ -16,8 +16,6 @@ static int8_t test_ins(uint8_t argc, const Menu::arg *argv);
static int8_t test_battery(uint8_t argc, const Menu::arg *argv);
static int8_t test_relay(uint8_t argc, const Menu::arg *argv);
static int8_t test_wp(uint8_t argc, const Menu::arg *argv);
static int8_t test_airspeed(uint8_t argc, const Menu::arg *argv);
static int8_t test_pressure(uint8_t argc, const Menu::arg *argv);
#if CONFIG_SONAR == ENABLED
static int8_t test_sonar(uint8_t argc, const Menu::arg *argv);
#endif
@ -59,8 +57,6 @@ static const struct Menu::command test_menu_commands[] PROGMEM = {
{"gps", test_gps},
{"rawgps", test_rawgps},
{"ins", test_ins},
{"airspeed", test_airspeed},
{"airpressure", test_pressure},
#if CONFIG_SONAR == ENABLED
{"sonartest", test_sonar},
#endif
@ -95,7 +91,7 @@ static void print_hit_enter()
static int8_t
test_eedump(uint8_t argc, const Menu::arg *argv)
{
int i, j;
intptr_t i, j;
// hexdump the EEPROM
for (i = 0; i < EEPROM_MAX_ADDR; i += 16) {
@ -330,13 +326,6 @@ test_wp(uint8_t argc, const Menu::arg *argv)
{
delay(1000);
// save the alitude above home option
if(g.RTL_altitude < 0){
Serial.printf_P(PSTR("Hold current altitude\n"));
}else{
Serial.printf_P(PSTR("Hold altitude of %dm\n"), (int)g.RTL_altitude/100);
}
Serial.printf_P(PSTR("%d waypoints\n"), (int)g.command_total);
Serial.printf_P(PSTR("Hit radius: %d\n"), (int)g.waypoint_radius);
Serial.printf_P(PSTR("Loiter radius: %d\n\n"), (int)g.loiter_radius);
@ -652,19 +641,6 @@ test_mag(uint8_t argc, const Menu::arg *argv)
#if HIL_MODE == HIL_MODE_DISABLED
static int8_t
test_airspeed(uint8_t argc, const Menu::arg *argv)
{
}
static int8_t
test_pressure(uint8_t argc, const Menu::arg *argv)
{
}
static int8_t
test_rawgps(uint8_t argc, const Menu::arg *argv)
{