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

APMrover v2.0c - tested with APM v2 full kit (Oilpan) - dualversion (IMUless + IMUfull) 

Signed-off-by: Jean-Louis Naudin <jlnaudin@gmail.com>
This commit is contained in:
Jean-Louis Naudin 2012-05-09 07:12:26 +02:00
parent 46ee92441e
commit 899e6e66b0
11 changed files with 97 additions and 122 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
APMrover2/APM_Config.h
View File

@ -10,11 +10,16 @@
#define CONFIG_APM_HARDWARE APM_HARDWARE_APM1
#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
#define CLI_SLIDER_ENABLED DISABLED
#define CLOSED_LOOP_NAV ENABLED
#define AUTO_WP_RADIUS ENABLED
#define TRACE 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

10
APMrover2/APM_Config_Rover.h
View File

@ -11,12 +11,14 @@
//#define GROUND_START_DELAY 1
#define AIRSPEED_SENSOR DISABLED
#define MAGNETOMETER ENABLED
#define LOGGING_ENABLED ENABLED
#define MAG_ORIENTATION AP_COMPASS_COMPONENTS_UP_PINS_FORWARD
#define PARAM_DECLINATION 0.18 // Paris
#if LITE == DISABLED
#define MAGNETOMETER ENABLED
#define LOGGING_ENABLED ENABLED
#define MAG_ORIENTATION AP_COMPASS_COMPONENTS_UP_PINS_FORWARD
#define PARAM_DECLINATION 0.18 // Paris
#endif
//////////////////////////////////////////////////////////////////////////////
// Serial port speeds.
//

67
APMrover2/APMrover2.pde
View File

@ -1,11 +1,11 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#define THISFIRMWARE "APMrover v2.0b JL NAUDIN" //New version of the APMrover for the APM v1 or APM v2 and magnetometer
#define THISFIRMWARE "APMrover v2.0c JL NAUDIN" //New version of the APMrover for the APM v1 or APM v2 and magnetometer
// This is a full version of Arduplane v2.32 specially adapted for a Rover by Jean-Louis Naudin (JLN)
/*
Authors: Doug Weibel, Jose Julio, Jordi Munoz, Jason Short, Andrew Tridgell, Randy Mackay, Pat Hickey, John Arne Birkeland, Olivier Adler
Authors: Doug Weibel, Jose Julio, Jordi Munoz, Jason Short, Andrew Tridgell, Randy Mackay, Pat Hickey, John Arne Birkeland, Olivier Adler, Jean-Louis Naudin
Thanks to: Chris Anderson, Michael Oborne, Paul Mather, Bill Premerlani, James Cohen, JB from rotorFX, Automatik, Fefenin, Peter Meister, Remzibi, Yury Smirnov, Sandro Benigno, Max Levine, Roberto Navoni, Lorenz Meier
Please contribute your ideas!
@ -16,7 +16,7 @@ License as published by the Free Software Foundation; either
version 2.1 of the License, or (at your option) any later version.
//
// JLN updates: last update 2012-05-01
// JLN updates: last update 2012-05-04
//
// DOLIST:
//
@ -24,6 +24,9 @@ version 2.1 of the License, or (at your option) any later version.
//-------------------------------------------------------------------------------------------------------------------------
// Dev Startup : 2012-04-21
//
// 2012-05-04: Added #define LITE ENABLED for the APM1280 or APM2560 CPU IMUless version
// 2012-05-03: Successful missions tests with a full APM2560 kit (GPS MT3329 + magnetometer HMC5883L)
// 2012-05-03: removing stick mixing in auto mode
// 2012-05-01: special update for rover about ground_course if compass is enabled
// 2012-04-30: Successfully tested in autonomous nav with a waypoints list recorded in live mode
// 2012-04-30: Now a full version for APM v1 or APM v2 with magnetometer
@ -586,7 +589,6 @@ 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 nav_yaw = 1; // deg * 100 : target yaw angle - used only for thermal hunt
static long altitude_estimate = 0; // for smoothing GPS output
////////////////////////////////////////////////////////////////////////////////
@ -643,8 +645,7 @@ static struct Location current_loc;
static struct Location next_WP;
// The location of the active waypoint in Guided mode.
static struct Location guided_WP;
static struct Location soarwp1_WP = {0,0,0}; // temp waypoint for Ridge Soaring
static struct Location soarwp2_WP = {0,0,0}; // temp waypoint for Ridge Soaring
// The location structure information from the Nav command being processed
static struct Location next_nav_command;
// The location structure information from the Non-Nav command being processed
@ -749,11 +750,12 @@ void loop()
if (millis() - perf_mon_timer > 20000) {
if (mainLoop_count != 0) {
#if LITE == DISABLED
if (g.log_bitmask & MASK_LOG_PM)
#if HIL_MODE != HIL_MODE_ATTITUDE
Log_Write_Performance();
#endif
#endif
resetPerfData();
}
}
@ -787,11 +789,14 @@ static void fast_loop()
gcs_update();
#endif
#if LITE == DISABLED
ahrs.update();
#endif
// uses the yaw from the DCM to give more accurate turns
calc_bearing_error();
#if LITE == DISABLED
# if HIL_MODE == HIL_MODE_DISABLED
if (g.log_bitmask & MASK_LOG_ATTITUDE_FAST)
Log_Write_Attitude((int)ahrs.roll_sensor, (int)ahrs.pitch_sensor, (uint16_t)ahrs.yaw_sensor);
@ -799,7 +804,7 @@ static void fast_loop()
if (g.log_bitmask & MASK_LOG_RAW)
Log_Write_Raw();
#endif
#endif
// inertial navigation
// ------------------
#if INERTIAL_NAVIGATION == ENABLED
@ -845,7 +850,8 @@ static void medium_loop()
update_GPS();
calc_gndspeed_undershoot();
}
#if LITE == DISABLED
#if HIL_MODE != HIL_MODE_ATTITUDE
if (g.compass_enabled && compass.read()) {
ahrs.set_compass(&compass);
@ -857,6 +863,7 @@ static void medium_loop()
ahrs.set_compass(NULL);
}
#endif
#endif
/*{
Serial.print(ahrs.roll_sensor, DEC); Serial.printf_P(PSTR("\t"));
Serial.print(ahrs.pitch_sensor, DEC); Serial.printf_P(PSTR("\t"));
@ -877,7 +884,7 @@ Serial.println(tempaccel.z, DEC);
if(g_gps->new_data){
g_gps->new_data = false;
dTnav = millis() - nav_loopTimer;
dTnav = millis() - nav_loopTimer;
nav_loopTimer = millis();
// calculate the plane's desired bearing
@ -901,7 +908,7 @@ Serial.println(tempaccel.z, DEC);
//-------------------------------------------------
case 3:
medium_loopCounter++;
#if LITE == DISABLED
#if HIL_MODE != HIL_MODE_ATTITUDE
if ((g.log_bitmask & MASK_LOG_ATTITUDE_MED) && !(g.log_bitmask & MASK_LOG_ATTITUDE_FAST))
Log_Write_Attitude((int)ahrs.roll_sensor, (int)ahrs.pitch_sensor, (uint16_t)ahrs.yaw_sensor);
@ -915,7 +922,7 @@ Serial.println(tempaccel.z, DEC);
if (g.log_bitmask & MASK_LOG_GPS)
Log_Write_GPS(g_gps->time, current_loc.lat, current_loc.lng, g_gps->altitude, current_loc.alt, (long) g_gps->ground_speed, g_gps->ground_course, g_gps->fix, g_gps->num_sats);
#endif
// send all requested output streams with rates requested
// between 5 and 45 Hz
gcs_data_stream_send(5,45);
@ -949,12 +956,13 @@ static void slow_loop()
check_long_failsafe();
superslow_loopCounter++;
if(superslow_loopCounter >=200) { // 200 = Execute every minute
#if LITE == DISABLED
#if HIL_MODE != HIL_MODE_ATTITUDE
if(g.compass_enabled) {
compass.save_offsets();
}
#endif
#endif
superslow_loopCounter = 0;
}
break;
@ -988,22 +996,31 @@ static void slow_loop()
#if USB_MUX_PIN > 0
check_usb_mux();
#endif
#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);
// 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);
#endif
break;
}
}
static void one_second_loop()
{
#if LITE == DISABLED
if (g.log_bitmask & MASK_LOG_CUR)
Log_Write_Current();
#endif
// send a heartbeat
gcs_send_message(MSG_HEARTBEAT);
gcs_data_stream_send(1,3);
}
static void update_GPS(void)
{
{ static uint16_t hdg;
g_gps->update();
update_GPS_light();
@ -1026,19 +1043,20 @@ static void update_GPS(void)
} 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
if (g.compass_enabled) {
// Set compass declination automatically
compass.set_initial_location(g_gps->latitude, g_gps->longitude);
}
#endif
ground_start_count = 0;
}
}
@ -1048,13 +1066,16 @@ static void update_GPS(void)
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) {
ground_course = (wrap_360(ToDeg(compass.heading) * 100));
} else {
ground_course = g_gps->ground_course;
hdg=(ahrs.yaw_sensor / 100) % 360;
ground_course = hdg * 100;
} else {
#endif
ground_course = abs(g_gps->ground_course);
#if LITE == DISABLED
}
#endif
// see if we've breached the geo-fence
geofence_check(false);
}

33
APMrover2/Attitude.pde
View File

@ -6,39 +6,10 @@
static void stabilize()
{
float ch1_inf = 1.0;
// Calculate desired servo output for the turn // Wheels Direction
// ---------------------------------------------
g.channel_roll.servo_out = nav_roll;
// Mix Stick input to allow users to override control surfaces
// -----------------------------------------------------------
if ((control_mode < FLY_BY_WIRE_A) ||
(ENABLE_STICK_MIXING == 1 &&
geofence_stickmixing() &&
control_mode > FLY_BY_WIRE_B &&
failsafe == FAILSAFE_NONE)) {
// TODO: use RC_Channel control_mix function?
ch1_inf = (float)g.channel_roll.radio_in - (float)g.channel_roll.radio_trim;
ch1_inf = fabs(ch1_inf);
ch1_inf = min(ch1_inf, 400.0);
ch1_inf = ((400.0 - ch1_inf) /400.0);
// scale the sensor input based on the stick input
// -----------------------------------------------
g.channel_roll.servo_out *= ch1_inf;
// Mix in stick inputs
// -------------------
g.channel_roll.servo_out += g.channel_roll.pwm_to_angle();
//Serial.printf_P(PSTR(" servo_out[CH_ROLL] "));
//Serial.println(servo_out[CH_ROLL],DEC);
}
g.channel_roll.servo_out = nav_roll;
g.channel_roll.servo_out = g.channel_roll.servo_out * g.turn_gain;
g.channel_rudder.servo_out = g.channel_roll.servo_out;
}

26
APMrover2/GCS_Mavlink.pde
View File

@ -114,7 +114,8 @@ static NOINLINE void send_attitude(mavlink_channel_t chan)
micros(),
ahrs.roll,
ahrs.pitch - radians(g.pitch_trim*0.01),
ahrs.yaw,
ToRad(ground_course/100.0),
//ahrs.yaw,
omega.x,
omega.y,
omega.z);
@ -317,9 +318,7 @@ static void NOINLINE send_location(mavlink_channel_t chan)
g_gps->ground_speed * rot.a.x, // X speed cm/s
g_gps->ground_speed * rot.b.x, // Y speed cm/s
g_gps->ground_speed * rot.c.x,
//g_gps->ground_course); // course in 1/100 degree
ground_course); // course in 1/100 degree
g_gps->ground_course); // course in 1/100 degree
}
static void NOINLINE send_nav_controller_output(mavlink_channel_t chan)
@ -333,7 +332,7 @@ static void NOINLINE send_nav_controller_output(mavlink_channel_t chan)
target_bearing / 100,
wp_distance,
altitude_error / 1.0e2,
airspeed_error,
groundspeed_error,
crosstrack_error);
}
@ -357,8 +356,7 @@ static void NOINLINE send_gps_raw(mavlink_channel_t chan)
g_gps->hdop,
65535,
g_gps->ground_speed, // cm/s
//g_gps->ground_course, // 1/100 degrees,
ground_course, // 1/100 degrees,
g_gps->ground_course, // 1/100 degrees,
g_gps->num_sats);
#else // MAVLINK10
@ -372,9 +370,7 @@ static void NOINLINE send_gps_raw(mavlink_channel_t chan)
g_gps->hdop,
0.0,
g_gps->ground_speed / 100.0,
//ground_course = (wrap_360(ToDeg(compass.heading) * 100));
//g_gps->ground_course / 100.0);
ground_course / 100.0);
g_gps->ground_course / 100.0);
#endif // MAVLINK10
}
@ -484,13 +480,13 @@ static void NOINLINE send_raw_imu1(mavlink_channel_t chan)
static void NOINLINE send_raw_imu2(mavlink_channel_t chan)
{
int32_t pressure = barometer.get_pressure();
int32_t pressure = 0;
mavlink_msg_scaled_pressure_send(
chan,
micros(),
pressure/100.0,
(pressure - g.ground_pressure)/100.0,
barometer.get_temperature());
0);
}
static void NOINLINE send_raw_imu3(mavlink_channel_t chan)
@ -502,8 +498,8 @@ static void NOINLINE send_raw_imu3(mavlink_channel_t chan)
mag_offsets.y,
mag_offsets.z,
compass.get_declination(),
barometer.get_raw_pressure(),
barometer.get_raw_temp(),
0,
0,
imu.gx(), imu.gy(), imu.gz(),
imu.ax(), imu.ay(), imu.az());
}
@ -1048,7 +1044,6 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
if (packet.param1 == 1 ||
packet.param2 == 1 ||
packet.param3 == 1) {
startup_IMU_ground(false);
}
if (packet.param4 == 1) {
trim_radio();
@ -1153,7 +1148,6 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
case MAV_ACTION_CALIBRATE_ACC:
case MAV_ACTION_CALIBRATE_PRESSURE:
case MAV_ACTION_REBOOT: // this is a rough interpretation
startup_IMU_ground(true);
result=1;
break;

3
APMrover2/Log.pde
View File

@ -1,5 +1,5 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#if LITE == DISABLED
#if LOGGING_ENABLED == ENABLED
// Code to Write and Read packets from DataFlash log memory
@ -694,3 +694,4 @@ static void Log_Write_Raw() {}
#endif // LOGGING_ENABLED
#endif

8
APMrover2/config.h
View File

@ -50,6 +50,10 @@
# define CONFIG_APM_HARDWARE APM_HARDWARE_APM1
#endif
#ifndef LITE
# define LITE DISABLED
#endif
#if defined( __AVR_ATmega1280__ )
#define CLI_ENABLED DISABLED
#define LOGGING_ENABLED DISABLED
@ -828,6 +832,10 @@
# define HIL_SERVOS DISABLED
#endif
#ifndef TRACE
# define TRACE DISABLED
#endif
// use this to completely disable the CLI
#ifndef CLI_ENABLED
# define CLI_ENABLED ENABLED

23
APMrover2/navigation.pde
View File

@ -77,31 +77,10 @@ static void calc_gndspeed_undershoot()
static void calc_bearing_error()
{
if(takeoff_complete == true || g.compass_enabled == true) {
/*
most of the time we use the yaw sensor for heading, even if
we don't have a compass. The yaw sensor is drift corrected
in the DCM library. We only use the gps ground course
directly if we haven't completed takeoff, as the yaw drift
correction won't have had a chance to kick in. Drift
correction using the GPS typically takes 10 seconds or so
for a 180 degree correction.
*/
bearing_error = nav_bearing - ahrs.yaw_sensor;
} else {
// TODO: we need to use the Yaw gyro for in between GPS reads,
// maybe as an offset from a saved gryo value.
bearing_error = nav_bearing - ground_course;
}
bearing_error = nav_bearing - ground_course;
bearing_error = wrap_180(bearing_error);
}
static void calc_altitude_error()
{
}
static long wrap_360(long error)
{
if (error > 36000) error -= 36000;

19
APMrover2/radio.pde
View File

@ -86,27 +86,8 @@ static void read_radio()
g.channel_throttle.set_pwm(APM_RC.InputCh(CH_3));
g.channel_rudder.set_pwm(APM_RC.InputCh(CH_4));
#if FLAPERON == ENABLED
// JLN update for true flaperons
if (control_mode == MANUAL) {
g.rc_5.set_pwm(APM_RC.InputCh(CH_5));
g.rc_6.set_pwm(APM_RC.InputCh(CH_6));
} else {
aileron1 = g.rc_5.radio_trim + (BOOL_TO_SIGN(-g.rc_5.get_reverse()) *g.channel_roll.angle_to_pwm());
aileron2 = g.rc_6.radio_trim + (BOOL_TO_SIGN(-g.rc_6.get_reverse()) *g.channel_roll.angle_to_pwm());
aileron1 = constrain(aileron1,(uint16_t)g.rc_5.radio_min,(uint16_t)g.rc_5.radio_max);
aileron2 = constrain(aileron2,(uint16_t)g.rc_6.radio_min,(uint16_t)g.rc_6.radio_max);
g.rc_5.set_pwm(aileron1);
g.rc_6.set_pwm(aileron2);
}
#else
g.rc_5.set_pwm(APM_RC.InputCh(CH_5));
g.rc_6.set_pwm(APM_RC.InputCh(CH_6));
#endif
g.rc_7.set_pwm(APM_RC.InputCh(CH_7));
g.rc_8.set_pwm(APM_RC.InputCh(CH_8));

3
APMrover2/sensors.pde
View File

@ -1,5 +1,5 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#if LITE == DISABLED
// Sensors are not available in HIL_MODE_ATTITUDE
#if HIL_MODE != HIL_MODE_ATTITUDE
@ -83,6 +83,7 @@ static void zero_airspeed(void)
}
#endif // HIL_MODE != HIL_MODE_ATTITUDE
#endif
static void read_battery(void)
{

22
APMrover2/system.pde
View File

@ -9,7 +9,9 @@ The init_ardupilot function processes everything we need for an in - air restart
#if CLI_ENABLED == ENABLED
// Functions called from the top-level menu
#if LITE == DISABLED
static int8_t process_logs(uint8_t argc, const Menu::arg *argv); // in Log.pde
#endif
static int8_t setup_mode(uint8_t argc, const Menu::arg *argv); // in setup.pde
static int8_t test_mode(uint8_t argc, const Menu::arg *argv); // in test.cpp
static int8_t planner_mode(uint8_t argc, const Menu::arg *argv); // in planner.pde
@ -33,7 +35,9 @@ static int8_t main_menu_help(uint8_t argc, const Menu::arg *argv)
static const struct Menu::command main_menu_commands[] PROGMEM = {
// command function called
// ======= ===============
#if LITE == DISABLED
{"logs", process_logs},
#endif
{"setup", setup_mode},
{"test", test_mode},
{"help", main_menu_help},
@ -155,6 +159,7 @@ static void init_ardupilot()
mavlink_system.sysid = g.sysid_this_mav;
#if LITE == DISABLED
#if LOGGING_ENABLED == ENABLED
DataFlash.Init(); // DataFlash log initialization
if (!DataFlash.CardInserted()) {
@ -168,6 +173,7 @@ static void init_ardupilot()
DataFlash.start_new_log();
}
#endif
#endif
#if HIL_MODE != HIL_MODE_ATTITUDE
@ -175,6 +181,7 @@ static void init_ardupilot()
adc.Init(&timer_scheduler); // APM ADC library initialization
#endif
#if LITE == DISABLED
barometer.init(&timer_scheduler);
if (g.compass_enabled==true) {
@ -189,7 +196,7 @@ static void init_ardupilot()
}
}
#endif
#endif
// Do GPS init
g_gps = &g_gps_driver;
g_gps->init(); // GPS Initialization
@ -257,9 +264,10 @@ static void init_ardupilot()
startup_ground();
#if LITE == DISABLED
if (g.log_bitmask & MASK_LOG_CMD)
Log_Write_Startup(TYPE_GROUNDSTART_MSG);
#endif
set_mode(MANUAL);
// set the correct flight mode
@ -286,12 +294,13 @@ static void startup_ground(void)
// -----------------------
demo_servos(1);
#if LITE == DISABLED
//IMU ground start
//------------------------
//
startup_IMU_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.
@ -370,8 +379,11 @@ static void set_mode(byte mode)
break;
}
#if LITE == DISABLED
if (g.log_bitmask & MASK_LOG_MODE)
Log_Write_Mode(control_mode);
#endif
}
static void check_long_failsafe()
@ -413,7 +425,7 @@ static void check_short_failsafe()
}
}
#if LITE == DISABLED
static void startup_IMU_ground(bool force_accel_level)
{
#if HIL_MODE != HIL_MODE_ATTITUDE
@ -455,7 +467,7 @@ static void startup_IMU_ground(bool force_accel_level)
digitalWrite(A_LED_PIN, LED_OFF);
digitalWrite(C_LED_PIN, LED_OFF);
}
#endif
static void update_GPS_light(void)
{