mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-22 16:48:29 -04:00
AP_Var integration continued
git-svn-id: https://arducopter.googlecode.com/svn/trunk@1697 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
jasonshort
parent
d036cb9360
commit
7acf9a15fc
@ -1,4 +1,4 @@
|
||||
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
|
||||
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
|
||||
|
||||
/*
|
||||
ArduCopterMega Version 0.1.3 Experimental
|
||||
@ -6,7 +6,8 @@ Authors: Jason Short
|
||||
Based on code and ideas from the Arducopter team: Jose Julio, Randy Mackay, Jani Hirvinen
|
||||
Thanks to: Chris Anderson, Mike Smith, Jordi Munoz, Doug Weibel, James Goppert, Benjamin Pelletier
|
||||
|
||||
This firmware is free software; you can redistribute it and / or
|
||||
|
||||
This firmware is free software; you can redistribute it and/or
|
||||
modify it under the terms of the GNU Lesser General Public
|
||||
License as published by the Free Software Foundation; either
|
||||
version 2.1 of the License, or (at your option) any later version.
|
||||
@ -26,19 +27,19 @@ version 2.1 of the License, or (at your option) any later version.
|
||||
#include <FastSerial.h>
|
||||
#include <AP_Common.h>
|
||||
#include <APM_RC.h> // ArduPilot Mega RC Library
|
||||
#include <RC_Channel.h> // RC Channel Library
|
||||
#include <AP_ADC.h> // ArduPilot Mega Analog to Digital Converter Library
|
||||
#include <AP_GPS.h> // ArduPilot GPS library
|
||||
#include <Wire.h> // Arduino I2C lib
|
||||
#include <APM_BMP085.h> // ArduPilot Mega BMP085 Library
|
||||
#include <DataFlash.h> // ArduPilot Mega Flash Memory Library
|
||||
#include <AP_Compass.h> // ArduPilot Mega Magnetometer Library
|
||||
#include <AP_ADC.h> // ArduPilot Mega Analog to Digital Converter Library
|
||||
#include <APM_BMP085.h> // ArduPilot Mega BMP085 Library
|
||||
#include <AP_Compass.h> // ArduPilot Mega Magnetometer Library
|
||||
#include <AP_Math.h> // ArduPilot Mega Vector/Matrix math Library
|
||||
#include <AP_IMU.h> // ArduPilot Mega IMU Library
|
||||
#include <AP_DCM.h> // ArduPilot Mega DCM Library
|
||||
#include <AP_DCM.h> // ArduPilot Mega DCM Library
|
||||
#include <PID.h> // ArduPilot Mega RC Library
|
||||
#include <GCS_MAVLink.h> // MAVLink GCS definitions
|
||||
#include <RC_Channel.h> // RC Channel Library
|
||||
|
||||
#include <GCS_MAVLink.h> // MAVLink GCS definitions
|
||||
|
||||
// Configuration
|
||||
#include "config.h"
|
||||
@ -57,9 +58,9 @@ version 2.1 of the License, or (at your option) any later version.
|
||||
// so there is not much of a penalty to defining ports that we don't
|
||||
// use.
|
||||
//
|
||||
FastSerialPort0(Serial); // FTDI/console
|
||||
FastSerialPort1(Serial1); // GPS port (except for GPS_PROTOCOL_IMU)
|
||||
FastSerialPort3(Serial3); // Telemetry port (optional, Standard and ArduPilot protocols only)
|
||||
FastSerialPort0(Serial); // FTDI/console
|
||||
FastSerialPort1(Serial1); // GPS port
|
||||
FastSerialPort3(Serial3); // Telemetry port
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
// Parameters
|
||||
@ -83,15 +84,12 @@ Parameters g;
|
||||
//
|
||||
|
||||
// All GPS access should be through this pointer.
|
||||
GPS *g_gps;
|
||||
|
||||
//#if HIL_MODE == HIL_MODE_NONE
|
||||
GPS *g_gps;
|
||||
|
||||
// real sensors
|
||||
AP_ADC_ADS7844 adc;
|
||||
APM_BMP085_Class APM_BMP085;
|
||||
AP_Compass_HMC5843 compass;
|
||||
|
||||
APM_BMP085_Class barometer;
|
||||
AP_Compass_HMC5843 compass(Parameters::k_param_compass);
|
||||
|
||||
// real GPS selection
|
||||
#if GPS_PROTOCOL == GPS_PROTOCOL_AUTO
|
||||
@ -135,14 +133,12 @@ byte control_mode = STABILIZE;
|
||||
boolean failsafe = false; // did our throttle dip below the failsafe value?
|
||||
boolean ch3_failsafe = false;
|
||||
byte oldSwitchPosition; // for remembering the control mode switch
|
||||
byte fbw_timer; // for limiting the execution of FBW input
|
||||
|
||||
const char *comma = ",";
|
||||
|
||||
//byte flight_modes[6];
|
||||
const char* flight_mode_strings[] = {
|
||||
"ACRO",
|
||||
"STABILIZE",
|
||||
"ACRO",
|
||||
"ALT_HOLD",
|
||||
"FBW",
|
||||
"AUTO",
|
||||
@ -166,15 +162,8 @@ const char* flight_mode_strings[] = {
|
||||
// Radio
|
||||
// -----
|
||||
int motor_out[4];
|
||||
//byte flight_mode_channel;
|
||||
//byte frame_type = PLUS_FRAME;
|
||||
|
||||
Vector3f omega;
|
||||
|
||||
|
||||
//float stabilize_dampener;
|
||||
//float hold_yaw_dampener;
|
||||
|
||||
// PIDs
|
||||
int max_stabilize_dampener; //
|
||||
int max_yaw_dampener; //
|
||||
@ -189,10 +178,10 @@ float nav_gain_scaler = 1; // Gain scaling for headwind/tailwind TO
|
||||
|
||||
// GPS variables
|
||||
// -------------
|
||||
byte ground_start_count = 5; // have we achieved first lock and set Home?
|
||||
const float t7 = 10000000.0; // used to scale GPS values for EEPROM storage
|
||||
float scaleLongUp = 1; // used to reverse longtitude scaling
|
||||
float scaleLongDown = 1; // used to reverse longtitude scaling
|
||||
byte ground_start_count = 5; // have we achieved first lock and set Home?
|
||||
|
||||
// Magnetometer variables
|
||||
// ----------------------
|
||||
@ -213,16 +202,9 @@ byte command_may_index; // current command memory location
|
||||
byte command_must_ID; // current command ID
|
||||
byte command_may_ID; // current command ID
|
||||
|
||||
//float altitude_gain; // in nav
|
||||
|
||||
float cos_roll_x;
|
||||
float sin_roll_y;
|
||||
|
||||
float cos_pitch_x;
|
||||
float sin_pitch_y;
|
||||
|
||||
float sin_yaw_y;
|
||||
float cos_yaw_x;
|
||||
float cos_roll_x, sin_roll_y;
|
||||
float cos_pitch_x, sin_pitch_y;
|
||||
float sin_yaw_y, cos_yaw_x;
|
||||
|
||||
// Airspeed
|
||||
// --------
|
||||
@ -234,11 +216,6 @@ float airspeed_error; // m / s * 100
|
||||
boolean motor_armed = false;
|
||||
boolean motor_auto_safe = false;
|
||||
|
||||
//byte throttle_failsafe_enabled;
|
||||
//int throttle_failsafe_value;
|
||||
//byte throttle_failsafe_action;
|
||||
//uint16_t log_bitmask;
|
||||
|
||||
// Location Errors
|
||||
// ---------------
|
||||
long bearing_error; // deg * 100 : 0 to 36000
|
||||
@ -260,7 +237,7 @@ float battery_voltage4 = LOW_VOLTAGE * 1.05; // Battery Voltage of cells 1 +
|
||||
float current_voltage = LOW_VOLTAGE * 1.05; // Battery Voltage of cells 1 + 2+3 + 4, initialized above threshold for filter
|
||||
float current_amps;
|
||||
float current_total;
|
||||
int milliamp_hours;
|
||||
//int milliamp_hours;
|
||||
//boolean current_enabled = false;
|
||||
|
||||
// Magnetometer variables
|
||||
@ -273,7 +250,6 @@ int milliamp_hours;
|
||||
//float mag_offset_y;
|
||||
//float mag_offset_z;
|
||||
//float mag_declination;
|
||||
//bool compass_enabled;
|
||||
|
||||
// Barometer Sensor variables
|
||||
// --------------------------
|
||||
@ -392,6 +368,7 @@ byte medium_count;
|
||||
|
||||
byte slow_loopCounter;
|
||||
byte superslow_loopCounter;
|
||||
byte fbw_timer; // for limiting the execution of FBW input
|
||||
|
||||
unsigned long nav_loopTimer; // used to track the elapsed ime for GPS nav
|
||||
unsigned long nav2_loopTimer; // used to track the elapsed ime for GPS nav
|
||||
@ -609,7 +586,6 @@ void medium_loop()
|
||||
gcs.update();
|
||||
}
|
||||
|
||||
|
||||
void slow_loop()
|
||||
{
|
||||
// This is the slow (3 1/3 Hz) loop pieces
|
||||
@ -617,17 +593,14 @@ void slow_loop()
|
||||
switch (slow_loopCounter){
|
||||
case 0:
|
||||
slow_loopCounter++;
|
||||
|
||||
superslow_loopCounter++;
|
||||
if(superslow_loopCounter == 30) {
|
||||
|
||||
// save current data to the flash
|
||||
if(superslow_loopCounter >= 200){
|
||||
if (g.log_bitmask & MASK_LOG_CUR)
|
||||
Log_Write_Current();
|
||||
|
||||
}else if(superslow_loopCounter >= 400) {
|
||||
compass.save_offsets();
|
||||
//eeprom_write_word((uint16_t *) EE_LAST_LOG_PAGE, DataFlash.GetWritePage());
|
||||
if(g.compass_enabled){
|
||||
compass.save_offsets();
|
||||
}
|
||||
superslow_loopCounter = 0;
|
||||
}
|
||||
break;
|
||||
@ -677,18 +650,17 @@ void update_GPS(void)
|
||||
g_gps->update();
|
||||
update_GPS_light();
|
||||
|
||||
// !!! comment out after testing
|
||||
//fake_out_gps();
|
||||
|
||||
//if (g_gps->new_data && g_gps->fix) {
|
||||
if (g_gps->new_data){
|
||||
send_message(MSG_LOCATION);
|
||||
if (g_gps->new_data && g_gps->fix) {
|
||||
// XXX We should be sending GPS data off one of the regular loops so that we send
|
||||
// no-GPS-fix data too
|
||||
#if GCS_PROTOCOL != GCS_PROTOCOL_MAVLINK
|
||||
gcs.send_message(MSG_LOCATION);
|
||||
#endif
|
||||
|
||||
// for performance
|
||||
// ---------------
|
||||
gps_fix_count++;
|
||||
|
||||
//Serial.printf("gs: %d\n", (int)ground_start_count);
|
||||
if(ground_start_count > 1){
|
||||
ground_start_count--;
|
||||
|
||||
@ -710,17 +682,13 @@ void update_GPS(void)
|
||||
// reset our nav loop timer
|
||||
nav_loopTimer = millis();
|
||||
init_home();
|
||||
|
||||
// init altitude
|
||||
current_loc.alt = g_gps->altitude;
|
||||
ground_start_count = 0;
|
||||
}
|
||||
}
|
||||
|
||||
/* disabled for now
|
||||
// baro_offset is an integrator for the g_gps altitude error
|
||||
baro_offset += altitude_gain * (float)(g_gps->altitude - current_loc.alt);
|
||||
*/
|
||||
|
||||
current_loc.lng = g_gps->longitude; // Lon * 10 * *7
|
||||
current_loc.lat = g_gps->latitude; // Lat * 10 * *7
|
||||
|
||||
|
||||
@ -1,5 +1,6 @@
|
||||
|
||||
void init_pids()
|
||||
void
|
||||
init_pids()
|
||||
{
|
||||
// create limits to how much dampening we'll allow
|
||||
// this creates symmetry with the P gain value preventing oscillations
|
||||
@ -9,7 +10,8 @@ void init_pids()
|
||||
}
|
||||
|
||||
|
||||
void control_nav_mixer()
|
||||
void
|
||||
control_nav_mixer()
|
||||
{
|
||||
// control +- 45° is mixed with the navigation request by the Autopilot
|
||||
// output is in degrees = target pitch and roll of copter
|
||||
@ -17,7 +19,8 @@ void control_nav_mixer()
|
||||
g.rc_2.servo_out = g.rc_2.control_mix(nav_pitch);
|
||||
}
|
||||
|
||||
void fbw_nav_mixer()
|
||||
void
|
||||
fbw_nav_mixer()
|
||||
{
|
||||
// control +- 45° is mixed with the navigation request by the Autopilot
|
||||
// output is in degrees = target pitch and roll of copter
|
||||
@ -25,7 +28,8 @@ void fbw_nav_mixer()
|
||||
g.rc_2.servo_out = nav_pitch;
|
||||
}
|
||||
|
||||
void output_stabilize_roll()
|
||||
void
|
||||
output_stabilize_roll()
|
||||
{
|
||||
float error, rate;
|
||||
int dampener;
|
||||
@ -48,7 +52,8 @@ void output_stabilize_roll()
|
||||
g.rc_1.servo_out -= constrain(dampener, -max_stabilize_dampener, max_stabilize_dampener); // limit to 1500 based on kP
|
||||
}
|
||||
|
||||
void output_stabilize_pitch()
|
||||
void
|
||||
output_stabilize_pitch()
|
||||
{
|
||||
float error, rate;
|
||||
int dampener;
|
||||
@ -80,7 +85,8 @@ clear_yaw_control()
|
||||
yaw_error = 0;
|
||||
}
|
||||
|
||||
void output_yaw_with_hold(boolean hold)
|
||||
void
|
||||
output_yaw_with_hold(boolean hold)
|
||||
{
|
||||
if(hold){
|
||||
// look to see if we have exited rate control properly - ie stopped turning
|
||||
@ -134,9 +140,8 @@ void output_yaw_with_hold(boolean hold)
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
void output_rate_roll()
|
||||
void
|
||||
output_rate_roll()
|
||||
{
|
||||
// rate control
|
||||
long rate = degrees(omega.x) * 100; // 3rad = 17188 , 6rad = 34377
|
||||
@ -147,7 +152,8 @@ void output_rate_roll()
|
||||
g.rc_1.servo_out = constrain(g.rc_1.servo_out, -2400, 2400); // limit to 2400
|
||||
}
|
||||
|
||||
void output_rate_pitch()
|
||||
void
|
||||
output_rate_pitch()
|
||||
{
|
||||
// rate control
|
||||
long rate = degrees(omega.y) * 100; // 3rad = 17188 , 6rad = 34377
|
||||
@ -158,29 +164,10 @@ void output_rate_pitch()
|
||||
g.rc_2.servo_out = constrain(g.rc_2.servo_out, -2400, 2400); // limit to 2400
|
||||
}
|
||||
|
||||
/*
|
||||
|
||||
g.rc_1.servo_out = g.rc_2.control_in;
|
||||
g.rc_2.servo_out = g.rc_2.control_in;
|
||||
|
||||
// Rate control through bias corrected gyro rates
|
||||
// omega is the raw gyro reading plus Omega_I, so it´s bias corrected
|
||||
g.rc_1.servo_out -= (omega.x * 5729.57795 * acro_dampener);
|
||||
g.rc_2.servo_out -= (omega.y * 5729.57795 * acro_dampener);
|
||||
|
||||
//Serial.printf("\trated out %d, omega ", g.rc_1.servo_out);
|
||||
//Serial.print((Omega[0] * 5729.57795 * stabilize_rate_roll_pitch), 3);
|
||||
|
||||
// Limit output
|
||||
g.rc_1.servo_out = constrain(g.rc_1.servo_out, -MAX_SERVO_OUTPUT, MAX_SERVO_OUTPUT);
|
||||
g.rc_2.servo_out = constrain(g.rc_2.servo_out, -MAX_SERVO_OUTPUT, MAX_SERVO_OUTPUT);
|
||||
*/
|
||||
//}
|
||||
|
||||
|
||||
// Zeros out navigation Integrators if we are changing mode, have passed a waypoint, etc.
|
||||
// Keeps outdated data out of our calculations
|
||||
void reset_I(void)
|
||||
void
|
||||
reset_I(void)
|
||||
{
|
||||
g.pid_nav_lat.reset_I();
|
||||
g.pid_nav_lon.reset_I();
|
||||
@ -190,3 +177,87 @@ void reset_I(void)
|
||||
|
||||
|
||||
|
||||
|
||||
/*************************************************************
|
||||
throttle control
|
||||
****************************************************************/
|
||||
|
||||
// user input:
|
||||
// -----------
|
||||
void output_manual_throttle()
|
||||
{
|
||||
g.rc_3.servo_out = (float)g.rc_3.control_in * angle_boost();
|
||||
}
|
||||
|
||||
// Autopilot
|
||||
// ---------
|
||||
void output_auto_throttle()
|
||||
{
|
||||
g.rc_3.servo_out = (float)nav_throttle * angle_boost();
|
||||
// make sure we never send a 0 throttle that will cut the motors
|
||||
g.rc_3.servo_out = max(g.rc_3.servo_out, 1);
|
||||
}
|
||||
|
||||
void calc_nav_throttle()
|
||||
{
|
||||
// limit error
|
||||
long error = constrain(altitude_error, -400, 400);
|
||||
|
||||
if(altitude_sensor == BARO) {
|
||||
float t = g.pid_baro_throttle.kP();
|
||||
|
||||
if(error > 0){ // go up
|
||||
g.pid_baro_throttle.kP(t);
|
||||
}else{ // go down
|
||||
g.pid_baro_throttle.kP(t/4.0);
|
||||
}
|
||||
|
||||
// limit output of throttle control
|
||||
nav_throttle = g.pid_baro_throttle.get_pid(error, delta_ms_fast_loop, 1.0);
|
||||
nav_throttle = g.throttle_cruise + constrain(nav_throttle, -30, 80);
|
||||
|
||||
g.pid_baro_throttle.kP(t);
|
||||
|
||||
} else {
|
||||
// SONAR
|
||||
nav_throttle = g.pid_sonar_throttle.get_pid(error, delta_ms_fast_loop, 1.0);
|
||||
|
||||
// limit output of throttle control
|
||||
nav_throttle = g.throttle_cruise + constrain(nav_throttle, -60, 100);
|
||||
}
|
||||
|
||||
nav_throttle = (nav_throttle + nav_throttle_old) >> 1;
|
||||
nav_throttle_old = nav_throttle;
|
||||
}
|
||||
|
||||
float angle_boost()
|
||||
{
|
||||
float temp = cos_pitch_x * cos_roll_x;
|
||||
temp = 2.0 - constrain(temp, .7, 1.0);
|
||||
return temp;
|
||||
}
|
||||
|
||||
|
||||
/*************************************************************
|
||||
yaw control
|
||||
****************************************************************/
|
||||
|
||||
void output_manual_yaw()
|
||||
{
|
||||
if(g.rc_3.control_in == 0){
|
||||
clear_yaw_control();
|
||||
} else {
|
||||
// Yaw control
|
||||
if(g.rc_4.control_in == 0){
|
||||
//clear_yaw_control();
|
||||
output_yaw_with_hold(true); // hold yaw
|
||||
}else{
|
||||
output_yaw_with_hold(false); // rate control yaw
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void auto_yaw()
|
||||
{
|
||||
output_yaw_with_hold(true); // hold yaw
|
||||
}
|
||||
|
||||
@ -34,7 +34,7 @@
|
||||
void save_EEPROM_alt_RTL(void)
|
||||
{
|
||||
g.RTL_altitude.save();
|
||||
|
||||
|
||||
}
|
||||
|
||||
void read_EEPROM_alt_RTL(void)
|
||||
@ -81,16 +81,16 @@ void read_EEPROM_PID(void)
|
||||
g.pid_acro_rate_roll.load_gains();
|
||||
g.pid_acro_rate_pitch.load_gains();
|
||||
g.pid_acro_rate_yaw.load_gains();
|
||||
|
||||
|
||||
g.pid_stabilize_roll.load_gains();
|
||||
g.pid_stabilize_pitch.load_gains();
|
||||
g.pid_yaw.load_gains();
|
||||
|
||||
|
||||
g.pid_nav_lon.load_gains();
|
||||
g.pid_nav_lat.load_gains();
|
||||
g.pid_baro_throttle.load_gains();
|
||||
g.pid_sonar_throttle.load_gains();
|
||||
|
||||
|
||||
// roll pitch
|
||||
g.stabilize_dampener.load();
|
||||
|
||||
@ -101,14 +101,15 @@ void read_EEPROM_PID(void)
|
||||
|
||||
void save_EEPROM_PID(void)
|
||||
{
|
||||
g.pid_acro_rate_roll.save_gains();
|
||||
|
||||
g.pid_acro_rate_roll.save_gains();
|
||||
g.pid_acro_rate_pitch.save_gains();
|
||||
g.pid_acro_rate_yaw.save_gains();
|
||||
|
||||
|
||||
g.pid_stabilize_roll.save_gains();
|
||||
g.pid_stabilize_pitch.save_gains();
|
||||
g.pid_yaw.save_gains();
|
||||
|
||||
|
||||
g.pid_nav_lon.save_gains();
|
||||
g.pid_nav_lat.save_gains();
|
||||
g.pid_baro_throttle.save_gains();
|
||||
@ -210,7 +211,7 @@ void save_EEPROM_pressure(void)
|
||||
void read_EEPROM_pressure(void)
|
||||
{
|
||||
g.ground_pressure.load();
|
||||
g.ground_temperature.load();
|
||||
g.ground_temperature.load();
|
||||
|
||||
// to prime the filter
|
||||
abs_pressure = g.ground_pressure;
|
||||
@ -231,7 +232,7 @@ void read_EEPROM_radio(void)
|
||||
}
|
||||
|
||||
void save_EEPROM_radio(void)
|
||||
{
|
||||
{
|
||||
g.rc_1.save_eeprom();
|
||||
g.rc_2.save_eeprom();
|
||||
g.rc_3.save_eeprom();
|
||||
@ -249,9 +250,9 @@ void read_EEPROM_throttle(void)
|
||||
g.throttle_min.load();
|
||||
g.throttle_max.load();
|
||||
g.throttle_cruise.load();
|
||||
g.throttle_failsafe_enabled.load();
|
||||
g.throttle_failsafe_action.load();
|
||||
g.throttle_failsafe_value.load();
|
||||
g.throttle_fs_enabled.load();
|
||||
g.throttle_fs_action.load();
|
||||
g.throttle_fs_value.load();
|
||||
}
|
||||
|
||||
void save_EEPROM_throttle(void)
|
||||
@ -259,9 +260,9 @@ void save_EEPROM_throttle(void)
|
||||
g.throttle_min.load();
|
||||
g.throttle_max.load();
|
||||
g.throttle_cruise.save();
|
||||
g.throttle_failsafe_enabled.load();
|
||||
g.throttle_failsafe_action.load();
|
||||
g.throttle_failsafe_value.load();
|
||||
g.throttle_fs_enabled.load();
|
||||
g.throttle_fs_action.load();
|
||||
g.throttle_fs_value.load();
|
||||
}
|
||||
|
||||
/********************************************************************************/
|
||||
@ -286,7 +287,7 @@ void read_EEPROM_flight_modes(void)
|
||||
void save_EEPROM_flight_modes(void)
|
||||
{
|
||||
g.flight_modes.save();
|
||||
|
||||
|
||||
}
|
||||
|
||||
/********************************************************************************/
|
||||
@ -298,8 +299,8 @@ read_EE_float(int address)
|
||||
byte bytes[4];
|
||||
float value;
|
||||
} _floatOut;
|
||||
|
||||
for (int i = 0; i < 4; i++)
|
||||
|
||||
for (int i = 0; i < 4; i++)
|
||||
_floatOut.bytes[i] = eeprom_read_byte((uint8_t *) (address + i));
|
||||
return _floatOut.value;
|
||||
}
|
||||
@ -310,9 +311,9 @@ void write_EE_float(float value, int address)
|
||||
byte bytes[4];
|
||||
float value;
|
||||
} _floatIn;
|
||||
|
||||
|
||||
_floatIn.value = value;
|
||||
for (int i = 0; i < 4; i++)
|
||||
for (int i = 0; i < 4; i++)
|
||||
eeprom_write_byte((uint8_t *) (address + i), _floatIn.bytes[i]);
|
||||
}
|
||||
*/
|
||||
@ -322,7 +323,7 @@ float
|
||||
read_EE_compressed_float(int address, byte places)
|
||||
{
|
||||
float scale = pow(10, places);
|
||||
|
||||
|
||||
int temp = eeprom_read_word((uint16_t *) address);
|
||||
return ((float)temp / scale);
|
||||
}
|
||||
|
||||
@ -58,6 +58,7 @@ public:
|
||||
k_param_ground_pressure,
|
||||
k_param_current,
|
||||
k_param_milliamp_hours,
|
||||
k_param_compass_enabled,
|
||||
k_param_compass,
|
||||
|
||||
//
|
||||
@ -83,9 +84,9 @@ public:
|
||||
k_param_rc_10,
|
||||
k_param_throttle_min,
|
||||
k_param_throttle_max,
|
||||
k_param_throttle_failsafe_enabled,
|
||||
k_param_throttle_failsafe_action,
|
||||
k_param_throttle_failsafe_value,
|
||||
k_param_throttle_fs_enabled,
|
||||
k_param_throttle_fs_action,
|
||||
k_param_throttle_fs_value,
|
||||
k_param_throttle_cruise,
|
||||
k_param_flight_mode_channel,
|
||||
k_param_flight_modes,
|
||||
@ -96,7 +97,7 @@ public:
|
||||
k_param_waypoint_mode = 220,
|
||||
k_param_waypoint_total,
|
||||
k_param_waypoint_index,
|
||||
k_param_command_must_index,
|
||||
k_param_command_must_index,
|
||||
k_param_waypoint_radius,
|
||||
k_param_loiter_radius,
|
||||
|
||||
@ -146,9 +147,9 @@ public:
|
||||
//
|
||||
AP_Int16 throttle_min;
|
||||
AP_Int16 throttle_max;
|
||||
AP_Int8 throttle_failsafe_enabled;
|
||||
AP_Int8 throttle_failsafe_action;
|
||||
AP_Int16 throttle_failsafe_value;
|
||||
AP_Int8 throttle_fs_enabled;
|
||||
AP_Int8 throttle_fs_action;
|
||||
AP_Int16 throttle_fs_value;
|
||||
AP_Int16 throttle_cruise;
|
||||
|
||||
// Flight modes
|
||||
@ -214,13 +215,13 @@ public:
|
||||
|
||||
crosstrack_gain (XTRACK_GAIN * 100, k_param_crosstrack_gain, PSTR("XTRK_GAIN")),
|
||||
crosstrack_entry_angle (XTRACK_ENTRY_ANGLE * 100, k_param_crosstrack_entry_angle, PSTR("XTRACK_ANGLE")),
|
||||
|
||||
|
||||
frame_type (FRAME_CONFIG, k_param_frame_type, PSTR("FRAME_CONFIG")),
|
||||
|
||||
|
||||
current_enabled (DISABLED, k_param_current, PSTR("CURRENT_ENABLE")),
|
||||
milliamp_hours (2100, k_param_milliamp_hours, PSTR("MAH")),
|
||||
compass_enabled (DISABLED, k_param_compass, PSTR("COMPASS_ENABLE")),
|
||||
|
||||
milliamp_hours (CURR_AMP_HOURS, k_param_milliamp_hours, PSTR("MAH")),
|
||||
compass_enabled (DISABLED, k_param_compass_enabled, PSTR("COMPASS_ENABLE")),
|
||||
|
||||
waypoint_mode (0, k_param_waypoint_mode, PSTR("WP_MODE")),
|
||||
waypoint_total (0, k_param_waypoint_total, PSTR("WP_TOTAL")),
|
||||
waypoint_index (0, k_param_waypoint_index, PSTR("WP_INDEX")),
|
||||
@ -228,18 +229,18 @@ public:
|
||||
waypoint_radius (WP_RADIUS_DEFAULT, k_param_waypoint_radius, PSTR("WP_RADIUS")),
|
||||
loiter_radius (LOITER_RADIUS_DEFAULT, k_param_loiter_radius, PSTR("LOITER_RADIUS")),
|
||||
|
||||
throttle_min (THROTTLE_MIN, k_param_throttle_min, PSTR("THR_MIN")),
|
||||
throttle_max (THROTTLE_MAX, k_param_throttle_max, PSTR("THR_MAX")),
|
||||
throttle_failsafe_enabled (THROTTLE_FAILSAFE, k_param_throttle_failsafe_enabled, PSTR("THR_FAILSAFE")),
|
||||
throttle_failsafe_action (THROTTLE_FAILSAFE_ACTION, k_param_throttle_failsafe_action, PSTR("THR_FS_ACTION")),
|
||||
throttle_failsafe_value (THROTTLE_FS_VALUE, k_param_throttle_failsafe_value, PSTR("THR_FS_VALUE")),
|
||||
throttle_cruise (THROTTLE_CRUISE, k_param_throttle_cruise, PSTR("TRIM_THROTTLE")),
|
||||
|
||||
throttle_min (THROTTLE_MIN, k_param_throttle_min, PSTR("THR_MIN")),
|
||||
throttle_max (THROTTLE_MAX, k_param_throttle_max, PSTR("THR_MAX")),
|
||||
throttle_fs_enabled (THROTTLE_FAILSAFE, k_param_throttle_fs_enabled, PSTR("THR_FAILSAFE")),
|
||||
throttle_fs_action (THROTTLE_FAILSAFE_ACTION, k_param_throttle_fs_action, PSTR("THR_FS_ACTION")),
|
||||
throttle_fs_value (THROTTLE_FS_VALUE, k_param_throttle_fs_value, PSTR("THR_FS_VALUE")),
|
||||
throttle_cruise (THROTTLE_CRUISE, k_param_throttle_cruise, PSTR("TRIM_THROTTLE")),
|
||||
|
||||
flight_mode_channel (FLIGHT_MODE_CHANNEL, k_param_flight_mode_channel, PSTR("FLIGHT_MODE_CH")),
|
||||
flight_modes (k_param_flight_modes, PSTR("FLIGHT_MODES")),
|
||||
|
||||
|
||||
pitch_max (PITCH_MAX_CENTIDEGREE, k_param_pitch_max, PSTR("PITCH_MAX_CENTIDEGREE")),
|
||||
|
||||
|
||||
log_bitmask (0, k_param_log_bitmask, PSTR("LOG_BITMASK")),
|
||||
ground_temperature (0, k_param_ground_temperature, PSTR("GND_TEMP")),
|
||||
ground_pressure (0, k_param_ground_pressure, PSTR("GND_ABS_PRESS")),
|
||||
@ -258,7 +259,6 @@ public:
|
||||
rc_camera_pitch (k_param_rc_9, PSTR("RC9_")),
|
||||
rc_camera_roll (k_param_rc_10, PSTR("RC10_")),
|
||||
|
||||
|
||||
// PID controller group key name initial P initial I initial D initial imax
|
||||
//--------------------------------------------------------------------------------------------------------------------------------------------------------------------
|
||||
pid_acro_rate_roll (k_param_pid_acro_rate_roll, PSTR("ACR_RLL_"), ACRO_RATE_ROLL_P, ACRO_RATE_ROLL_I, ACRO_RATE_ROLL_D, ACRO_RATE_ROLL_IMAX * 100),
|
||||
|
||||
@ -25,12 +25,12 @@
|
||||
///
|
||||
/// DO NOT EDIT THIS INCLUDE - if you want to make a local change, make that
|
||||
/// change in your local copy of APM_Config.h.
|
||||
///
|
||||
///
|
||||
#include "APM_Config.h" // <== THIS INCLUDE, DO NOT EDIT IT
|
||||
///
|
||||
/// DO NOT EDIT THIS INCLUDE - if you want to make a local change, make that
|
||||
/// change in your local copy of APM_Config.h.
|
||||
///
|
||||
///
|
||||
|
||||
// Just so that it's completely clear...
|
||||
#define ENABLED 1
|
||||
@ -67,7 +67,7 @@
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////
|
||||
// FRAME_CONFIG
|
||||
//
|
||||
//
|
||||
#ifndef FRAME_CONFIG
|
||||
# define FRAME_CONFIG PLUS_FRAME
|
||||
#endif
|
||||
@ -122,6 +122,9 @@
|
||||
#ifndef CURR_AMP_DIV_RATIO
|
||||
# define CURR_AMP_DIV_RATIO 30.35
|
||||
#endif
|
||||
#ifndef CURR_AMP_HOURS
|
||||
# define CURR_AMP_HOURS 2000
|
||||
#endif
|
||||
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////
|
||||
@ -257,7 +260,7 @@
|
||||
# define ACRO_RATE_PITCH_D 0.0
|
||||
#endif
|
||||
#ifndef ACRO_RATE_PITCH_IMAX
|
||||
# define ACRO_RATE_PITCH_IMAX 20
|
||||
# define ACRO_RATE_PITCH_IMAX 20
|
||||
#endif
|
||||
#define ACRO_RATE_PITCH_IMAX_CENTIDEGREE ACRO_RATE_PITCH_IMAX * 100
|
||||
|
||||
@ -426,8 +429,8 @@
|
||||
#ifndef XTRACK_ENTRY_ANGLE
|
||||
# define XTRACK_ENTRY_ANGLE 30 // deg
|
||||
#endif
|
||||
# define XTRACK_GAIN_SCALED XTRACK_GAIN * 100
|
||||
# define XTRACK_ENTRY_ANGLE_CENTIDEGREE XTRACK_ENTRY_ANGLE * 100
|
||||
//# define XTRACK_GAIN_SCALED XTRACK_GAIN * 100
|
||||
//# define XTRACK_ENTRY_ANGLE_CENTIDEGREE XTRACK_ENTRY_ANGLE * 100
|
||||
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////
|
||||
|
||||
@ -97,9 +97,9 @@
|
||||
|
||||
// Auto Pilot modes
|
||||
// ----------------
|
||||
#define ACRO 0 // rate control
|
||||
#define STABILIZE 1 // hold level position
|
||||
#define ALT_HOLD 2 // AUTO control
|
||||
#define STABILIZE 0 // hold level position
|
||||
#define ACRO 1 // rate control
|
||||
#define ALT_HOLD 2 // AUTO control
|
||||
#define FBW 3 // AUTO control
|
||||
#define AUTO 4 // AUTO control
|
||||
#define POSITION_HOLD 5 // Hold a single location
|
||||
|
||||
@ -1,154 +0,0 @@
|
||||
|
||||
/*************************************************************
|
||||
throttle control
|
||||
****************************************************************/
|
||||
|
||||
// user input:
|
||||
// -----------
|
||||
void output_manual_throttle()
|
||||
{
|
||||
g.rc_3.servo_out = (float)g.rc_3.control_in * angle_boost();
|
||||
}
|
||||
|
||||
// Autopilot
|
||||
// ---------
|
||||
void output_auto_throttle()
|
||||
{
|
||||
g.rc_3.servo_out = (float)nav_throttle * angle_boost();
|
||||
// make sure we never send a 0 throttle that will cut the motors
|
||||
g.rc_3.servo_out = max(g.rc_3.servo_out, 1);
|
||||
}
|
||||
|
||||
void calc_nav_throttle()
|
||||
{
|
||||
// limit error
|
||||
long error = constrain(altitude_error, -400, 400);
|
||||
|
||||
if(altitude_sensor == BARO) {
|
||||
float t = g.pid_baro_throttle.kP();
|
||||
|
||||
if(error > 0){ // go up
|
||||
g.pid_baro_throttle.kP(t);
|
||||
}else{ // go down
|
||||
g.pid_baro_throttle.kP(t/4.0);
|
||||
}
|
||||
|
||||
// limit output of throttle control
|
||||
nav_throttle = g.pid_baro_throttle.get_pid(error, delta_ms_fast_loop, 1.0);
|
||||
nav_throttle = g.throttle_cruise + constrain(nav_throttle, -30, 80);
|
||||
|
||||
g.pid_baro_throttle.kP(t);
|
||||
|
||||
} else {
|
||||
// SONAR
|
||||
nav_throttle = g.pid_sonar_throttle.get_pid(error, delta_ms_fast_loop, 1.0);
|
||||
|
||||
// limit output of throttle control
|
||||
nav_throttle = g.throttle_cruise + constrain(nav_throttle, -60, 100);
|
||||
}
|
||||
|
||||
nav_throttle = (nav_throttle + nav_throttle_old) >> 1;
|
||||
nav_throttle_old = nav_throttle;
|
||||
}
|
||||
|
||||
float angle_boost()
|
||||
{
|
||||
float temp = cos_pitch_x * cos_roll_x;
|
||||
temp = 2.0 - constrain(temp, .7, 1.0);
|
||||
return temp;
|
||||
}
|
||||
|
||||
|
||||
/*************************************************************
|
||||
yaw control
|
||||
****************************************************************/
|
||||
|
||||
void output_manual_yaw()
|
||||
{
|
||||
if(g.rc_3.control_in == 0){
|
||||
clear_yaw_control();
|
||||
} else {
|
||||
// Yaw control
|
||||
if(g.rc_4.control_in == 0){
|
||||
//clear_yaw_control();
|
||||
output_yaw_with_hold(true); // hold yaw
|
||||
}else{
|
||||
output_yaw_with_hold(false); // rate control yaw
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void auto_yaw()
|
||||
{
|
||||
output_yaw_with_hold(true); // hold yaw
|
||||
}
|
||||
|
||||
/*************************************************************
|
||||
picth and roll control
|
||||
****************************************************************/
|
||||
|
||||
|
||||
/*// how hard to tilt towards the target
|
||||
// -----------------------------------
|
||||
void calc_nav_pid()
|
||||
{
|
||||
// how hard to pitch to target
|
||||
|
||||
nav_angle = g.pid_nav.get_pid(wp_distance * 100, dTnav, 1.0);
|
||||
nav_angle = constrain(nav_angle, -pitch_max, pitch_max);
|
||||
}
|
||||
|
||||
// distribute the pitch angle based on our orientation
|
||||
// ---------------------------------------------------
|
||||
void calc_nav_pitch()
|
||||
{
|
||||
// how hard to pitch to target
|
||||
|
||||
long angle = wrap_360(nav_bearing - dcm.yaw_sensor);
|
||||
|
||||
bool rev = false;
|
||||
float roll_out;
|
||||
|
||||
if(angle > 18000){
|
||||
angle -= 18000;
|
||||
rev = true;
|
||||
}
|
||||
|
||||
roll_out = abs(angle - 18000);
|
||||
roll_out = (9000.0 - roll_out) / 9000.0;
|
||||
roll_out = (rev) ? roll_out : -roll_out;
|
||||
|
||||
nav_pitch = (float)nav_angle * roll_out;
|
||||
}
|
||||
|
||||
// distribute the roll angle based on our orientation
|
||||
// --------------------------------------------------
|
||||
void calc_nav_roll()
|
||||
{
|
||||
long angle = wrap_360(nav_bearing - dcm.yaw_sensor);
|
||||
|
||||
bool rev = false;
|
||||
float roll_out;
|
||||
|
||||
if(angle > 18000){
|
||||
angle -= 18000;
|
||||
rev = true;
|
||||
}
|
||||
|
||||
roll_out = abs(angle - 9000);
|
||||
roll_out = (9000.0 - roll_out) / 9000.0;
|
||||
roll_out = (rev) ? -roll_out : roll_out;
|
||||
|
||||
nav_roll = (float)nav_angle * roll_out;
|
||||
}
|
||||
*/
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
@ -5,20 +5,20 @@ void init_pressure_ground(void)
|
||||
{
|
||||
for(int i = 0; i < 300; i++){ // We take some readings...
|
||||
delay(20);
|
||||
APM_BMP085.Read(); // Get initial data from absolute pressure sensor
|
||||
ground_pressure = (ground_pressure * 9l + APM_BMP085.Press) / 10l;
|
||||
ground_temperature = (ground_temperature * 9 + APM_BMP085.Temp) / 10;
|
||||
barometer.Read(); // Get initial data from absolute pressure sensor
|
||||
ground_pressure = (ground_pressure * 9l + barometer.Press) / 10l;
|
||||
ground_temperature = (ground_temperature * 9 + barometer.Temp) / 10;
|
||||
}
|
||||
abs_pressure = APM_BMP085.Press;
|
||||
abs_pressure = barometer.Press;
|
||||
}
|
||||
|
||||
void read_barometer(void){
|
||||
float x, scaling, temp;
|
||||
|
||||
APM_BMP085.Read(); // Get new data from absolute pressure sensor
|
||||
barometer.Read(); // Get new data from absolute pressure sensor
|
||||
|
||||
//abs_pressure = (abs_pressure + APM_BMP085.Press) >> 1; // Small filtering
|
||||
abs_pressure = ((float)abs_pressure * .7) + ((float)APM_BMP085.Press * .3); // large filtering
|
||||
//abs_pressure = (abs_pressure + barometer.Press) >> 1; // Small filtering
|
||||
abs_pressure = ((float)abs_pressure * .7) + ((float)barometer.Press * .3); // large filtering
|
||||
scaling = (float)ground_pressure / (float)abs_pressure;
|
||||
temp = ((float)ground_temperature / 10.f) + 273.15;
|
||||
x = log(scaling) * temp * 29271.267f;
|
||||
|
||||
@ -96,7 +96,7 @@ setup_factory(uint8_t argc, const Menu::arg *argv)
|
||||
Serial.printf_P(PSTR("\nFACTORY RESET complete - please reset APM to continue"));
|
||||
for (;;) {
|
||||
}
|
||||
|
||||
|
||||
// note, cannot actually return here
|
||||
return(0);
|
||||
|
||||
@ -491,7 +491,7 @@ setup_current(uint8_t argc, const Menu::arg *argv)
|
||||
save_EEPROM_mag();
|
||||
|
||||
} else if(argv[1].i > 10){
|
||||
milliamp_hours = argv[1].i;
|
||||
g.milliamp_hours = argv[1].i;
|
||||
|
||||
} else {
|
||||
Serial.printf_P(PSTR("\nOptions:[on, off, mAh]\n"));
|
||||
@ -515,8 +515,7 @@ setup_mag_offset(uint8_t argc, const Menu::arg *argv)
|
||||
|
||||
compass.init(); // Initialization
|
||||
compass.set_orientation(MAGORIENTATION); // set compass's orientation on aircraft
|
||||
compass.set_offsets(0, 0, 0); // set offsets to account for surrounding interference
|
||||
compass.set_declination(ToRad(DECLINATION)); // set local difference between magnetic north and true north
|
||||
//compass.set_offsets(0, 0, 0); // set offsets to account for surrounding interference
|
||||
//int counter = 0;
|
||||
float _min[3], _max[3], _offset[3];
|
||||
|
||||
@ -621,7 +620,7 @@ default_frame()
|
||||
void
|
||||
default_current()
|
||||
{
|
||||
milliamp_hours = 2000;
|
||||
g.milliamp_hours = 2000;
|
||||
g.current_enabled.set(false);
|
||||
save_EEPROM_current();
|
||||
}
|
||||
@ -644,9 +643,9 @@ default_throttle()
|
||||
g.throttle_min = THROTTLE_MIN;
|
||||
g.throttle_max = THROTTLE_MAX;
|
||||
g.throttle_cruise = THROTTLE_CRUISE;
|
||||
g.throttle_failsafe_enabled = THROTTLE_FAILSAFE;
|
||||
g.throttle_failsafe_action = THROTTLE_FAILSAFE_ACTION;
|
||||
g.throttle_failsafe_value = THROTTLE_FS_VALUE;
|
||||
g.throttle_fs_enabled = THROTTLE_FAILSAFE;
|
||||
g.throttle_fs_action = THROTTLE_FAILSAFE_ACTION;
|
||||
g.throttle_fs_value = THROTTLE_FS_VALUE;
|
||||
save_EEPROM_throttle();
|
||||
}
|
||||
|
||||
@ -742,7 +741,7 @@ default_gains()
|
||||
|
||||
save_EEPROM_PID();
|
||||
Serial.printf("EL8R %4.2f\n ",g.pid_stabilize_roll.kP());
|
||||
|
||||
|
||||
}
|
||||
|
||||
|
||||
@ -759,7 +758,7 @@ void report_current()
|
||||
print_divider();
|
||||
print_enabled(g.current_enabled.get());
|
||||
|
||||
Serial.printf_P(PSTR("mah: %d"),milliamp_hours);
|
||||
Serial.printf_P(PSTR("mah: %d"),g.milliamp_hours);
|
||||
print_blanks(1);
|
||||
}
|
||||
|
||||
@ -865,8 +864,8 @@ void report_throttle()
|
||||
(int)g.throttle_min,
|
||||
(int)g.throttle_max,
|
||||
(int)g.throttle_cruise,
|
||||
(int)g.throttle_failsafe_enabled,
|
||||
(int)g.throttle_failsafe_value);
|
||||
(int)g.throttle_fs_enabled,
|
||||
(int)g.throttle_fs_value);
|
||||
print_blanks(1);
|
||||
}
|
||||
|
||||
@ -896,7 +895,7 @@ void report_compass()
|
||||
// mag declination
|
||||
Serial.printf_P(PSTR("Mag Delination: %4.4f\n"),
|
||||
degrees(compass.get_declination()));
|
||||
|
||||
|
||||
Vector3f offsets = compass.get_offsets();
|
||||
|
||||
// mag offsets
|
||||
|
||||
@ -116,7 +116,7 @@ void init_ardupilot()
|
||||
init_rc_out(); // sets up the timer libs
|
||||
init_camera();
|
||||
adc.Init(); // APM ADC library initialization
|
||||
APM_BMP085.Init(); // APM Abs Pressure sensor initialization
|
||||
barometer.Init(); // APM Abs Pressure sensor initialization
|
||||
DataFlash.Init(); // DataFlash log initialization
|
||||
|
||||
// Do GPS init
|
||||
@ -130,8 +130,8 @@ void init_ardupilot()
|
||||
#else
|
||||
gcs.init(&Serial);
|
||||
#endif
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
if(g.compass_enabled)
|
||||
@ -386,7 +386,7 @@ init_compass()
|
||||
dcm.set_compass(&compass);
|
||||
bool junkbool = compass.init();
|
||||
compass.set_orientation(MAGORIENTATION); // set compass's orientation on aircraft
|
||||
Vector3f junkvector = compass.get_offsets(); // load offsets to account for airframe magnetic interference
|
||||
Vector3f junkvector = compass.get_offsets(); // load offsets to account for airframe magnetic interference
|
||||
}
|
||||
|
||||
|
||||
|
||||
@ -184,7 +184,7 @@ test_failsafe(uint8_t argc, const Menu::arg *argv)
|
||||
fail_test++;
|
||||
}
|
||||
|
||||
if(g.throttle_failsafe_enabled && g.rc_3.get_failsafe()){
|
||||
if(g.throttle_fs_enabled && g.rc_3.get_failsafe()){
|
||||
Serial.printf_P(PSTR("THROTTLE FAILSAFE ACTIVATED: %d, "), g.rc_3.radio_in);
|
||||
Serial.println(flight_mode_strings[readSwitch()]);
|
||||
fail_test++;
|
||||
@ -237,6 +237,7 @@ test_stabilize(uint8_t argc, const Menu::arg *argv)
|
||||
medium_loopCounter = 0;
|
||||
}
|
||||
}
|
||||
|
||||
// for trim features
|
||||
read_trim_switch();
|
||||
|
||||
|
||||
Loading…
Reference in New Issue
Block a user