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

Merge branch 'master' of https://code.google.com/p/ardupilot-mega

This commit is contained in:
Chris Anderson 2012-02-15 16:26:40 -08:00
parent 91aab0f074 a43ee36bee
commit 3cc986c472
105 changed files with 5255 additions and 1489 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
ArduCopter/APM_Config.h
View File

@ -63,11 +63,29 @@
// Ensure the defined file exists and is in the arducopter directory
#define USERHOOK_VARIABLES "UserVariables.h"
// to enable, set to 1
// to disable, set to 0
#define AUTO_THROTTLE_HOLD 1
//# define LOGGING_ENABLED DISABLED
//# define LOGGING_ENABLED DISABLED
// Custom channel config - Expert Use Only.
// this for defining your own MOT_n to CH_n mapping.
// Overrides defaults (for APM1 or APM2) found in config_channels.h
// MOT_n variables are used by the Frame mixing code. You must define
// MOT_1 through MOT_m where m is the number of motors on your frame.
// CH_n variables are used for RC output. These can be CH_1 through CH_8,
// and CH_10 or CH_12.
// Sample channel config. Must define all MOT_ chanels used by
// your FRAME_TYPE.
// #define CONFIG_CHANNELS CHANNEL_CONFIG_CUSTOM
// #define MOT_1 CH_6
// #define MOT_2 CH_3
// #define MOT_3 CH_2
// #define MOT_4 CH_5
// #define MOT_5 CH_1
// #define MOT_6 CH_4
// #define MOT_7 CH_7
// #define MOT_8 CH_8

87
ArduCopter/ArduCopter.pde
View File

@ -179,7 +179,7 @@ static AP_Int8 *flight_modes = &g.flight_mode1;
AP_Baro_MS5611 barometer;
#endif
AP_Compass_HMC5843 compass(Parameters::k_param_compass);
AP_Compass_HMC5843 compass;
#endif
#ifdef OPTFLOW_ENABLED
@ -219,7 +219,7 @@ AP_InertialSensor_MPU6000 ins( CONFIG_MPU6000_CHIP_SELECT_PIN );
#else
AP_InertialSensor_Oilpan ins(&adc);
#endif
AP_IMU_INS imu(&ins, Parameters::k_param_IMU_calibration);
AP_IMU_INS imu(&ins);
AP_DCM dcm(&imu, g_gps);
AP_TimerProcess timer_scheduler;
@ -258,8 +258,8 @@ AP_TimerProcess timer_scheduler;
////////////////////////////////////////////////////////////////////////////////
// GCS selection
////////////////////////////////////////////////////////////////////////////////
GCS_MAVLINK gcs0(Parameters::k_param_streamrates_port0);
GCS_MAVLINK gcs3(Parameters::k_param_streamrates_port3);
GCS_MAVLINK gcs0;
GCS_MAVLINK gcs3;
////////////////////////////////////////////////////////////////////////////////
// SONAR selection
@ -652,14 +652,17 @@ static int32_t nav_roll;
// The Commanded pitch from the autopilot. negative Pitch means go forward.
static int32_t nav_pitch;
// The desired bank towards North (Positive) or South (Negative)
static int32_t auto_roll;
static int32_t auto_pitch;
// Don't be fooled by the fact that Pitch is reversed from Roll in its sign!
static int16_t nav_lat;
// The desired bank towards East (Positive) or West (Negative)
static int16_t nav_lon;
// This may go away, but for now I'm tracking the desired bank before we apply the Wind compensation I term
// This is mainly for debugging
static int16_t nav_lat_p;
static int16_t nav_lon_p;
//static int16_t nav_lat_p;
//static int16_t nav_lon_p;
// The Commanded ROll from the autopilot based on optical flow sensor.
static int32_t of_roll = 0;
@ -1294,8 +1297,8 @@ static void update_GPS(void)
}else{
// after 12 reads we guess we may have lost GPS signal, stop navigating
// we have lost GPS signal for a moment. Reduce our error to avoid flyaways
nav_roll >>= 1;
nav_pitch >>= 1;
auto_roll >>= 1;
auto_pitch >>= 1;
}
if (g_gps->new_data && g_gps->fix) {
@ -1423,12 +1426,28 @@ void update_roll_pitch_mode(void)
g.rc_2.servo_out = get_stabilize_pitch(g.rc_2.control_in);
break;
/* case ROLL_PITCH_AUTO:
// apply SIMPLE mode transform
if(do_simple && new_radio_frame){
update_simple_mode();
}
// mix in user control with Nav control
control_roll = g.rc_1.control_mix(nav_roll);
control_pitch = g.rc_2.control_mix(nav_pitch);
g.rc_1.servo_out = get_stabilize_roll(control_roll);
g.rc_2.servo_out = get_stabilize_pitch(control_pitch);
break;
*/
case ROLL_PITCH_AUTO:
// apply SIMPLE mode transform
if(do_simple && new_radio_frame){
update_simple_mode();
}
// mix in user control with Nav control
nav_roll += constrain(wrap_180(auto_roll - nav_roll), -g.auto_slew_rate.get(), g.auto_slew_rate.get()); // 40 deg a second
nav_pitch += constrain(wrap_180(auto_pitch - nav_pitch), -g.auto_slew_rate.get(), g.auto_slew_rate.get()); // 40 deg a second
control_roll = g.rc_1.control_mix(nav_roll);
control_pitch = g.rc_2.control_mix(nav_pitch);
g.rc_1.servo_out = get_stabilize_roll(control_roll);
@ -1665,7 +1684,10 @@ static void update_navigation()
if((wp_distance <= g.waypoint_radius) || check_missed_wp()){
// if auto_land_timer value > 0, we are set to trigger auto_land after 20 seconds
set_mode(LOITER);
auto_land_timer = millis();
if(g.rtl_land_enabled || failsafe)
auto_land_timer = millis();
else
auto_land_timer = 0;
break;
}
@ -1914,118 +1936,119 @@ adjust_altitude()
static void tuning(){
tuning_value = (float)g.rc_6.control_in / 1000.0;
g.rc_6.set_range(g.radio_tuning_low,g.radio_tuning_high); // 0 to 1
switch(g.radio_tuning){
case CH6_DAMP:
g.rc_6.set_range(0,300); // 0 to 1
g.stablize_d.set(tuning_value);
g.stabilize_d.set(tuning_value);
//g.rc_6.set_range(0,60); // 0 to 1
//tuning_value = (float)g.rc_6.control_in / 100000.0;
//g.rc_6.set_range(0,1000); // 0 to 1
//g.pid_rate_roll.kD(tuning_value);
//g.pid_rate_pitch.kD(tuning_value);
break;
case CH6_STABILIZE_KP:
g.rc_6.set_range(0,8000); // 0 to 8
//g.rc_6.set_range(0,8000); // 0 to 8
g.pi_stabilize_roll.kP(tuning_value);
g.pi_stabilize_pitch.kP(tuning_value);
break;
case CH6_STABILIZE_KI:
g.rc_6.set_range(0,300); // 0 to .3
tuning_value = (float)g.rc_6.control_in / 1000.0;
//g.rc_6.set_range(0,300); // 0 to .3
//tuning_value = (float)g.rc_6.control_in / 1000.0;
g.pi_stabilize_roll.kI(tuning_value);
g.pi_stabilize_pitch.kI(tuning_value);
break;
case CH6_RATE_KP:
g.rc_6.set_range(40,300); // 0 to .3
//g.rc_6.set_range(40,300); // 0 to .3
g.pid_rate_roll.kP(tuning_value);
g.pid_rate_pitch.kP(tuning_value);
break;
case CH6_RATE_KI:
g.rc_6.set_range(0,300); // 0 to .3
//g.rc_6.set_range(0,500); // 0 to .5
g.pid_rate_roll.kI(tuning_value);
g.pid_rate_pitch.kI(tuning_value);
break;
case CH6_YAW_KP:
g.rc_6.set_range(0,1000);
//g.rc_6.set_range(0,1000);
g.pi_stabilize_yaw.kP(tuning_value);
break;
case CH6_YAW_RATE_KP:
g.rc_6.set_range(0,1000);
//g.rc_6.set_range(0,1000);
g.pid_rate_yaw.kP(tuning_value);
break;
case CH6_THROTTLE_KP:
g.rc_6.set_range(0,1000); // 0 to 1
//g.rc_6.set_range(0,1000); // 0 to 1
g.pid_throttle.kP(tuning_value);
break;
case CH6_TOP_BOTTOM_RATIO:
g.rc_6.set_range(800,1000); // .8 to 1
//g.rc_6.set_range(800,1000); // .8 to 1
g.top_bottom_ratio = tuning_value;
break;
case CH6_RELAY:
g.rc_6.set_range(0,1000);
//g.rc_6.set_range(0,1000);
if (g.rc_6.control_in > 525) relay.on();
if (g.rc_6.control_in < 475) relay.off();
break;
case CH6_TRAVERSE_SPEED:
g.rc_6.set_range(0,1000);
//g.rc_6.set_range(0,1000);
g.waypoint_speed_max = g.rc_6.control_in;
break;
case CH6_LOITER_P:
g.rc_6.set_range(0,2000);
//g.rc_6.set_range(0,2000);
g.pi_loiter_lat.kP(tuning_value);
g.pi_loiter_lon.kP(tuning_value);
break;
case CH6_NAV_P:
g.rc_6.set_range(0,4000);
//g.rc_6.set_range(0,4000);
g.pid_nav_lat.kP(tuning_value);
g.pid_nav_lon.kP(tuning_value);
break;
case CH6_NAV_I:
g.rc_6.set_range(0,500);
//g.rc_6.set_range(0,500);
g.pid_nav_lat.kI(tuning_value);
g.pid_nav_lon.kI(tuning_value);
break;
#if FRAME_CONFIG == HELI_FRAME
case CH6_HELI_EXTERNAL_GYRO:
g.rc_6.set_range(1000,2000);
//g.rc_6.set_range(1000,2000);
g.heli_ext_gyro_gain = tuning_value * 1000;
break;
#endif
case CH6_THR_HOLD_KP:
g.rc_6.set_range(0,1000); // 0 to 1
//g.rc_6.set_range(0,1000); // 0 to 1
g.pi_alt_hold.kP(tuning_value);
break;
case CH6_OPTFLOW_KP:
g.rc_6.set_range(0,5000); // 0 to 5
//g.rc_6.set_range(0,5000); // 0 to 5
g.pid_optflow_roll.kP(tuning_value);
g.pid_optflow_pitch.kP(tuning_value);
break;
case CH6_OPTFLOW_KI:
g.rc_6.set_range(0,10000); // 0 to 10
//g.rc_6.set_range(0,10000); // 0 to 10
g.pid_optflow_roll.kI(tuning_value);
g.pid_optflow_pitch.kI(tuning_value);
break;
case CH6_OPTFLOW_KD:
g.rc_6.set_range(0,200); // 0 to 0.2
//g.rc_6.set_range(0,200); // 0 to 0.2
g.pid_optflow_roll.kD(tuning_value);
g.pid_optflow_pitch.kD(tuning_value);
break;
@ -2131,4 +2154,4 @@ static void update_auto_yaw()
auto_yaw = target_bearing;
}
// MAV_ROI_NONE = basic Yaw hold
}
}

8
ArduCopter/Attitude.pde
View File

@ -83,7 +83,7 @@ get_stabilize_yaw(int32_t target_angle)
static int
get_acro_roll(int32_t target_rate)
{
target_rate = target_rate * g.pi_stabilize_roll.kP();
target_rate = target_rate * g.acro_p;
target_rate = constrain(target_rate, -10000, 10000);
return get_rate_roll(target_rate);
}
@ -91,7 +91,7 @@ get_acro_roll(int32_t target_rate)
static int
get_acro_pitch(int32_t target_rate)
{
target_rate = target_rate * g.pi_stabilize_pitch.kP();
target_rate = target_rate * g.acro_p;
target_rate = constrain(target_rate, -10000, 10000);
return get_rate_pitch(target_rate);
}
@ -115,7 +115,7 @@ get_rate_roll(int32_t target_rate)
target_rate = g.pid_rate_roll.get_pid(target_rate, G_Dt);
// Dampening
target_rate -= constrain((current_rate - last_rate) * g.stablize_d, -500, 500);
target_rate -= constrain((current_rate - last_rate) * g.stabilize_d, -500, 500);
last_rate = current_rate;
// output control:
@ -133,7 +133,7 @@ get_rate_pitch(int32_t target_rate)
target_rate = g.pid_rate_pitch.get_pid(target_rate, G_Dt);
// Dampening
target_rate -= constrain((current_rate - last_rate) * g.stablize_d, -500, 500);
target_rate -= constrain((current_rate - last_rate) * g.stabilize_d, -500, 500);
last_rate = current_rate;
// output control:

11
ArduCopter/GCS.h
View File

@ -108,7 +108,7 @@ protected:
class GCS_MAVLINK : public GCS_Class
{
public:
GCS_MAVLINK(AP_Var::Key key);
GCS_MAVLINK();
void update(void);
void init(FastSerial *port);
void send_message(enum ap_message id);
@ -118,13 +118,16 @@ public:
void queued_param_send();
void queued_waypoint_send();
static const struct AP_Param::GroupInfo var_info[];
private:
void handleMessage(mavlink_message_t * msg);
/// Perform queued sending operations
///
AP_Var *_queued_parameter; ///< next parameter to be sent in queue
AP_Param *_queued_parameter; ///< next parameter to be sent in queue
enum ap_var_type _queued_parameter_type; ///< type of the next parameter
uint16_t _queued_parameter_token; ///AP_Param token for next() call
uint16_t _queued_parameter_index; ///< next queued parameter's index
uint16_t _queued_parameter_count; ///< saved count of parameters for queued send
@ -139,7 +142,6 @@ private:
uint16_t _count_parameters(); ///< count reportable parameters
uint16_t _parameter_count; ///< cache of reportable parameters
AP_Var *_find_parameter(uint16_t index); ///< find a reportable parameter by index
mavlink_channel_t chan;
uint16_t packet_drops;
@ -163,7 +165,6 @@ private:
uint16_t waypoint_receive_timeout; // milliseconds
// data stream rates
AP_Var_group _group;
AP_Int16 streamRateRawSensors;
AP_Int16 streamRateExtendedStatus;
AP_Int16 streamRateRCChannels;

123
ArduCopter/GCS_Mavlink.pde
View File

@ -416,7 +416,7 @@ static bool mavlink_try_send_message(mavlink_channel_t chan, enum ap_message id,
CHECK_PAYLOAD_SIZE(PARAM_VALUE);
if (chan == MAVLINK_COMM_0) {
gcs0.queued_param_send();
} else {
} else if (gcs3.initialised) {
gcs3.queued_param_send();
}
break;
@ -523,27 +523,23 @@ void mavlink_send_text(mavlink_channel_t chan, gcs_severity severity, const char
}
}
const AP_Param::GroupInfo GCS_MAVLINK::var_info[] PROGMEM = {
AP_GROUPINFO("RAW_SENS", 0, GCS_MAVLINK, streamRateRawSensors),
AP_GROUPINFO("EXT_STAT", 1, GCS_MAVLINK, streamRateExtendedStatus),
AP_GROUPINFO("RC_CHAN", 2, GCS_MAVLINK, streamRateRCChannels),
AP_GROUPINFO("RAW_CTRL", 3, GCS_MAVLINK, streamRateRawController),
AP_GROUPINFO("POSITION", 4, GCS_MAVLINK, streamRatePosition),
AP_GROUPINFO("EXTRA1", 5, GCS_MAVLINK, streamRateExtra1),
AP_GROUPINFO("EXTRA2", 6, GCS_MAVLINK, streamRateExtra2),
AP_GROUPINFO("EXTRA3", 7, GCS_MAVLINK, streamRateExtra3),
AP_GROUPEND
};
GCS_MAVLINK::GCS_MAVLINK(AP_Var::Key key) :
GCS_MAVLINK::GCS_MAVLINK() :
packet_drops(0),
// parameters
// note, all values not explicitly initialised here are zeroed
waypoint_send_timeout(1000), // 1 second
waypoint_receive_timeout(1000), // 1 second
// stream rates
_group (key, key == Parameters::k_param_streamrates_port0 ? PSTR("SR0_"): PSTR("SR3_")),
// AP_VAR //ref //index, default, name
streamRateRawSensors (&_group, 0, 0, PSTR("RAW_SENS")),
streamRateExtendedStatus (&_group, 1, 0, PSTR("EXT_STAT")),
streamRateRCChannels (&_group, 2, 0, PSTR("RC_CHAN")),
streamRateRawController (&_group, 3, 0, PSTR("RAW_CTRL")),
streamRatePosition (&_group, 4, 0, PSTR("POSITION")),
streamRateExtra1 (&_group, 5, 0, PSTR("EXTRA1")),
streamRateExtra2 (&_group, 6, 0, PSTR("EXTRA2")),
streamRateExtra3 (&_group, 7, 0, PSTR("EXTRA3"))
waypoint_receive_timeout(1000) // 1 second
{
}
@ -857,12 +853,14 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
break;
case MAV_ACTION_STORAGE_READ:
AP_Var::load_all();
// we load all variables at startup, and
// save on each mavlink set
result=1;
break;
case MAV_ACTION_STORAGE_WRITE:
AP_Var::save_all();
// this doesn't make any sense, as we save
// all settings as they come in
result=1;
break;
@ -1136,7 +1134,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
// Start sending parameters - next call to ::update will kick the first one out
_queued_parameter = AP_Var::first();
_queued_parameter = AP_Param::first(&_queued_parameter_token, &_queued_parameter_type);
_queued_parameter_index = 0;
_queued_parameter_count = _count_parameters();
break;
@ -1379,8 +1377,8 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
case MAVLINK_MSG_ID_PARAM_SET: // 23
{
AP_Var *vp;
AP_Meta_class::Type_id var_type;
AP_Param *vp;
enum ap_var_type var_type;
// decode
mavlink_param_set_t packet;
@ -1396,8 +1394,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
key[ONBOARD_PARAM_NAME_LENGTH] = 0;
// find the requested parameter
vp = AP_Var::find(key);
vp = AP_Param::find(key, &var_type);
if ((NULL != vp) && // exists
!isnan(packet.param_value) && // not nan
!isinf(packet.param_value)) { // not inf
@ -1407,38 +1404,27 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
// next lower integer value.
float rounding_addition = 0.01;
// fetch the variable type ID
var_type = vp->meta_type_id();
// handle variables with standard type IDs
if (var_type == AP_Var::k_typeid_float) {
if (var_type == AP_PARAM_FLOAT) {
((AP_Float *)vp)->set_and_save(packet.param_value);
} else if (var_type == AP_PARAM_INT32) {
#if LOGGING_ENABLED == ENABLED
Log_Write_Data(1, (float)((AP_Float *)vp)->get());
Log_Write_Data(1, ((AP_Int32 *)vp)->get());
#endif
} else if (var_type == AP_Var::k_typeid_float16) {
((AP_Float16 *)vp)->set_and_save(packet.param_value);
#if LOGGING_ENABLED == ENABLED
Log_Write_Data(2, (float)((AP_Float *)vp)->get());
#endif
} else if (var_type == AP_Var::k_typeid_int32) {
if (packet.param_value < 0) rounding_addition = -rounding_addition;
((AP_Int32 *)vp)->set_and_save(packet.param_value+rounding_addition);
} else if (var_type == AP_PARAM_INT16) {
#if LOGGING_ENABLED == ENABLED
Log_Write_Data(3, (int32_t)((AP_Float *)vp)->get());
Log_Write_Data(3, (int32_t)((AP_Int16 *)vp)->get());
#endif
} else if (var_type == AP_Var::k_typeid_int16) {
if (packet.param_value < 0) rounding_addition = -rounding_addition;
((AP_Int16 *)vp)->set_and_save(packet.param_value+rounding_addition);
} else if (var_type == AP_PARAM_INT8) {
#if LOGGING_ENABLED == ENABLED
Log_Write_Data(4, (int32_t)((AP_Float *)vp)->get());
Log_Write_Data(4, (int32_t)((AP_Int8 *)vp)->get());
#endif
} else if (var_type == AP_Var::k_typeid_int8) {
if (packet.param_value < 0) rounding_addition = -rounding_addition;
((AP_Int8 *)vp)->set_and_save(packet.param_value+rounding_addition);
#if LOGGING_ENABLED == ENABLED
Log_Write_Data(5, (int32_t)((AP_Float *)vp)->get());
#endif
} else {
// we don't support mavlink set on this parameter
break;
@ -1451,7 +1437,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
mavlink_msg_param_value_send(
chan,
(int8_t *)key,
vp->cast_to_float(),
vp->cast_to_float(var_type),
_count_parameters(),
-1); // XXX we don't actually know what its index is...
@ -1630,44 +1616,17 @@ GCS_MAVLINK::_count_parameters()
{
// if we haven't cached the parameter count yet...
if (0 == _parameter_count) {
AP_Var *vp;
AP_Param *vp;
uint16_t token;
vp = AP_Var::first();
vp = AP_Param::first(&token, NULL);
do {
// if a parameter responds to cast_to_float then we are going to be able to report it
if (!isnan(vp->cast_to_float())) {
_parameter_count++;
}
} while (NULL != (vp = vp->next()));
} while (NULL != (vp = AP_Param::next_scalar(&token, NULL)));
}
return _parameter_count;
}
AP_Var *
GCS_MAVLINK::_find_parameter(uint16_t index)
{
AP_Var *vp;
vp = AP_Var::first();
while (NULL != vp) {
// if the parameter is reportable
if (!(isnan(vp->cast_to_float()))) {
// if we have counted down to the index we want
if (0 == index) {
// return the parameter
return vp;
}
// count off this parameter, as it is reportable but not
// the one we want
index--;
}
// and move to the next parameter
vp = vp->next();
}
return NULL;
}
/**
* @brief Send the next pending parameter, called from deferred message
* handling code
@ -1678,18 +1637,16 @@ GCS_MAVLINK::queued_param_send()
// Check to see if we are sending parameters
if (NULL == _queued_parameter) return;
AP_Var *vp;
AP_Param *vp;
float value;
// copy the current parameter and prepare to move to the next
vp = _queued_parameter;
_queued_parameter = _queued_parameter->next();
// if the parameter can be cast to float, report it here and break out of the loop
value = vp->cast_to_float();
if (!isnan(value)) {
char param_name[ONBOARD_PARAM_NAME_LENGTH]; /// XXX HACK
memset(param_name, 0, sizeof(param_name));
value = vp->cast_to_float(_queued_parameter_type);
char param_name[ONBOARD_PARAM_NAME_LENGTH];
vp->copy_name(param_name, sizeof(param_name));
mavlink_msg_param_value_send(
@ -1699,9 +1656,9 @@ GCS_MAVLINK::queued_param_send()
_queued_parameter_count,
_queued_parameter_index);
_queued_parameter = AP_Param::next_scalar(&_queued_parameter_token, &_queued_parameter_type);
_queued_parameter_index++;
}
}
/**
* @brief Send the next pending waypoint, called from deferred message

20
ArduCopter/Log.pde
View File

@ -79,7 +79,7 @@ print_log_menu(void)
if (num_logs == 0) {
Serial.printf_P(PSTR("\nNo logs\n\n"));
}else{
Serial.printf_P(PSTR("\n%d logs\n"), num_logs);
Serial.printf_P(PSTR("\n%u logs\n"), (unsigned)num_logs);
for(int i=num_logs;i>=1;i--) {
int last_log_start = log_start, last_log_end = log_end;
@ -219,7 +219,7 @@ void print_latlon(BetterStream *s, int32_t lat_or_lon)
{
int32_t dec_portion = lat_or_lon / T7;
int32_t frac_portion = labs(lat_or_lon - dec_portion*T7);
s->printf("%ld.%07ld",dec_portion,frac_portion);
s->printf("%ld.%07ld",(long)dec_portion,(long)frac_portion);
}
// Write an GPS packet. Total length : 31 bytes
@ -602,13 +602,13 @@ static void Log_Read_Control_Tuning()
Serial.printf_P(PSTR("CTUN, "));
for(int8_t i = 1; i < 11; i++ ){
for(uint8_t i = 1; i < 11; i++ ){
temp = DataFlash.ReadInt();
Serial.printf("%d, ", temp);
Serial.printf("%d, ", (int)temp);
}
// read 11
temp = DataFlash.ReadInt();
Serial.printf("%d\n", temp);
Serial.printf("%d\n", (int)temp);
}
// Write a performance monitoring packet. Total length : 9 bytes
@ -636,11 +636,11 @@ static void Log_Read_Performance()
//1 2 3 4 5
Serial.printf_P(PSTR("PM, %d, %d, %d, %d, %d\n"),
temp1,
temp2,
temp3,
temp4,
temp5);
(int)temp1,
(int)temp2,
(int)temp3,
(int)temp4,
(int)temp5);
}
// Write a command processing packet. Total length : 21 bytes

3
ArduCopter/Makefile
View File

@ -21,6 +21,9 @@ heli:
apm2:
make -f Makefile EXTRAFLAGS="-DCONFIG_APM_HARDWARE=APM_HARDWARE_APM2"
apm2hexa:
make -f Makefile EXTRAFLAGS="-DCONFIG_APM_HARDWARE=APM_HARDWARE_APM2 -DFRAME_CONFIG=HEXA_FRAME"
apm2beta:
make -f Makefile EXTRAFLAGS="-DCONFIG_APM_HARDWARE=APM_HARDWARE_APM2 -DAPM2_BETA_HARDWARE"

253
ArduCopter/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 = 115;
static const uint16_t k_format_version = 116;
// The parameter software_type is set up solely for ground station use
// and identifies the software type (eg ArduPilotMega versus ArduCopterMega)
@ -83,8 +83,8 @@ public:
// 110: Telemetry control
//
k_param_streamrates_port0 = 110,
k_param_streamrates_port3,
k_param_gcs0 = 110,
k_param_gcs3,
k_param_sysid_this_mav,
k_param_sysid_my_gcs,
k_param_serial3_baud,
@ -100,14 +100,16 @@ public:
k_param_pack_capacity,
k_param_compass_enabled,
k_param_compass,
k_param_sonar,
k_param_sonar_enabled,
k_param_frame_orientation,
k_param_top_bottom_ratio,
k_param_optflow_enabled,
k_param_low_voltage,
k_param_ch7_option,
k_param_auto_slew_rate,
k_param_sonar_type,
k_param_super_simple, //155
k_param_rtl_land_enabled,
//
// 160: Navigation parameters
@ -128,8 +130,8 @@ public:
k_param_rc_6,
k_param_rc_7,
k_param_rc_8,
k_param_rc_9,
k_param_rc_10,
k_param_rc_camera_pitch,// rc_9
k_param_rc_camera_roll, // rc_10
k_param_throttle_min,
k_param_throttle_max,
k_param_throttle_fs_enabled,
@ -138,13 +140,15 @@ public:
k_param_throttle_cruise,
k_param_esc_calibrate,
k_param_radio_tuning,
k_param_radio_tuning_high,
k_param_radio_tuning_low,
k_param_camera_pitch_gain,
k_param_camera_roll_gain,
//
// 210: flight modes
// 200: flight modes
//
k_param_flight_mode1,
k_param_flight_mode1 = 200,
k_param_flight_mode2,
k_param_flight_mode3,
k_param_flight_mode4,
@ -153,9 +157,9 @@ public:
k_param_simple_modes,
//
// 220: Waypoint data
// 210: Waypoint data
//
k_param_waypoint_mode = 220,
k_param_waypoint_mode = 210,
k_param_command_total,
k_param_command_index,
k_param_command_nav_index,
@ -164,10 +168,11 @@ public:
k_param_waypoint_speed_max,
//
// 235: PI/D Controllers
// 220: PI/D Controllers
//
k_param_stabilize_d = 234,
k_param_pid_rate_roll = 235,
k_param_stabilize_d = 220,
k_param_acro_p,
k_param_pid_rate_roll,
k_param_pid_rate_pitch,
k_param_pid_rate_yaw,
k_param_pi_stabilize_roll,
@ -175,12 +180,14 @@ public:
k_param_pi_stabilize_yaw,
k_param_pi_loiter_lat,
k_param_pi_loiter_lon,
k_param_pid_loiter_rate_lat,
k_param_pid_loiter_rate_lon,
k_param_pid_nav_lat,
k_param_pid_nav_lon,
k_param_pi_alt_hold,
k_param_pid_throttle,
k_param_pid_optflow_roll,
k_param_pid_optflow_pitch, // 250
k_param_pid_optflow_pitch,
// 254,255: reserved
};
@ -207,6 +214,7 @@ public:
AP_Int8 optflow_enabled;
AP_Float low_voltage;
AP_Int8 super_simple;
AP_Int8 rtl_land_enabled;
// Waypoints
@ -248,10 +256,12 @@ public:
AP_Int8 esc_calibrate;
AP_Int8 radio_tuning;
AP_Int16 radio_tuning_high;
AP_Int16 radio_tuning_low;
AP_Int8 frame_orientation;
AP_Float top_bottom_ratio;
AP_Int8 ch7_option;
AP_Int16 auto_slew_rate;
#if FRAME_CONFIG == HELI_FRAME
// Heli
@ -281,12 +291,16 @@ public:
AP_Float camera_pitch_gain;
AP_Float camera_roll_gain;
AP_Float stablize_d;
AP_Float stabilize_d;
// PI/D controllers
AP_Float acro_p;
AC_PID pid_rate_roll;
AC_PID pid_rate_pitch;
AC_PID pid_rate_yaw;
AC_PID pid_loiter_rate_lat;
AC_PID pid_loiter_rate_lon;
AC_PID pid_nav_lat;
AC_PID pid_nav_lon;
@ -301,129 +315,118 @@ public:
APM_PI pi_stabilize_yaw;
APM_PI pi_alt_hold;
uint8_t junk;
// Note: keep initializers here in the same order as they are declared above.
Parameters() :
// variable default key name
//-------------------------------------------------------------------------------------------------------------------
format_version (k_format_version, k_param_format_version, PSTR("SYSID_SW_MREV")),
software_type (k_software_type, k_param_software_type, PSTR("SYSID_SW_TYPE")),
// variable default
//----------------------------------------
format_version (k_format_version),
software_type (k_software_type),
sysid_this_mav (MAV_SYSTEM_ID, k_param_sysid_this_mav, PSTR("SYSID_THISMAV")),
sysid_my_gcs (255, k_param_sysid_my_gcs, PSTR("SYSID_MYGCS")),
serial3_baud (SERIAL3_BAUD/1000, k_param_serial3_baud, PSTR("SERIAL3_BAUD")),
sysid_this_mav (MAV_SYSTEM_ID),
sysid_my_gcs (255),
serial3_baud (SERIAL3_BAUD/1000),
RTL_altitude (ALT_HOLD_HOME * 100, k_param_RTL_altitude, PSTR("ALT_HOLD_RTL")),
sonar_enabled (DISABLED, k_param_sonar, PSTR("SONAR_ENABLE")),
sonar_type (AP_RANGEFINDER_MAXSONARXL, k_param_sonar_type, PSTR("SONAR_TYPE")),
battery_monitoring (DISABLED, k_param_battery_monitoring, PSTR("BATT_MONITOR")),
volt_div_ratio (VOLT_DIV_RATIO, k_param_volt_div_ratio, PSTR("VOLT_DIVIDER")),
curr_amp_per_volt (CURR_AMP_PER_VOLT, k_param_curr_amp_per_volt, PSTR("AMP_PER_VOLT")),
input_voltage (INPUT_VOLTAGE, k_param_input_voltage, PSTR("INPUT_VOLTS")),
pack_capacity (HIGH_DISCHARGE, k_param_pack_capacity, PSTR("BATT_CAPACITY")),
compass_enabled (MAGNETOMETER, k_param_compass_enabled, PSTR("MAG_ENABLE")),
optflow_enabled (OPTFLOW, k_param_optflow_enabled, PSTR("FLOW_ENABLE")),
low_voltage (LOW_VOLTAGE, k_param_low_voltage, PSTR("LOW_VOLT")),
super_simple (SUPER_SIMPLE, k_param_super_simple, PSTR("SUPER_SIMPLE")),
RTL_altitude (ALT_HOLD_HOME * 100),
sonar_enabled (DISABLED),
sonar_type (AP_RANGEFINDER_MAXSONARXL),
battery_monitoring (DISABLED),
volt_div_ratio (VOLT_DIV_RATIO),
curr_amp_per_volt (CURR_AMP_PER_VOLT),
input_voltage (INPUT_VOLTAGE),
pack_capacity (HIGH_DISCHARGE),
compass_enabled (MAGNETOMETER),
optflow_enabled (OPTFLOW),
low_voltage (LOW_VOLTAGE),
super_simple (SUPER_SIMPLE),
rtl_land_enabled (RTL_AUTO_LAND),
waypoint_mode (0, k_param_waypoint_mode, PSTR("WP_MODE")),
command_total (0, k_param_command_total, PSTR("WP_TOTAL")),
command_index (0, k_param_command_index, PSTR("WP_INDEX")),
command_nav_index (0, k_param_command_nav_index, PSTR("WP_MUST_INDEX")),
waypoint_radius (WP_RADIUS_DEFAULT * 100, k_param_waypoint_radius, PSTR("WP_RADIUS")),
loiter_radius (LOITER_RADIUS, k_param_loiter_radius, PSTR("WP_LOITER_RAD")),
waypoint_speed_max (WAYPOINT_SPEED_MAX, k_param_waypoint_speed_max, PSTR("WP_SPEED_MAX")),
crosstrack_gain (CROSSTRACK_GAIN, k_param_crosstrack_gain, PSTR("XTRK_GAIN_SC")),
auto_land_timeout (AUTO_LAND_TIME * 1000, k_param_auto_land_timeout, PSTR("AUTO_LAND")),
waypoint_mode (0),
command_total (0),
command_index (0),
command_nav_index (0),
waypoint_radius (WP_RADIUS_DEFAULT * 100),
loiter_radius (LOITER_RADIUS),
waypoint_speed_max (WAYPOINT_SPEED_MAX),
crosstrack_gain (CROSSTRACK_GAIN),
auto_land_timeout (AUTO_LAND_TIME * 1000),
throttle_min (0, k_param_throttle_min, PSTR("THR_MIN")),
throttle_max (1000, 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")),
throttle_min (0),
throttle_max (1000),
throttle_fs_enabled (THROTTLE_FAILSAFE),
throttle_fs_action (THROTTLE_FAILSAFE_ACTION),
throttle_fs_value (THROTTLE_FS_VALUE),
throttle_cruise (THROTTLE_CRUISE),
flight_mode1 (FLIGHT_MODE_1, k_param_flight_mode1, PSTR("FLTMODE1")),
flight_mode2 (FLIGHT_MODE_2, k_param_flight_mode2, PSTR("FLTMODE2")),
flight_mode3 (FLIGHT_MODE_3, k_param_flight_mode3, PSTR("FLTMODE3")),
flight_mode4 (FLIGHT_MODE_4, k_param_flight_mode4, PSTR("FLTMODE4")),
flight_mode5 (FLIGHT_MODE_5, k_param_flight_mode5, PSTR("FLTMODE5")),
flight_mode6 (FLIGHT_MODE_6, k_param_flight_mode6, PSTR("FLTMODE6")),
simple_modes (0, k_param_simple_modes, PSTR("SIMPLE")),
flight_mode1 (FLIGHT_MODE_1),
flight_mode2 (FLIGHT_MODE_2),
flight_mode3 (FLIGHT_MODE_3),
flight_mode4 (FLIGHT_MODE_4),
flight_mode5 (FLIGHT_MODE_5),
flight_mode6 (FLIGHT_MODE_6),
simple_modes (0),
log_bitmask (DEFAULT_LOG_BITMASK, k_param_log_bitmask, PSTR("LOG_BITMASK")),
log_last_filenumber (0, k_param_log_last_filenumber, PSTR("LOG_LASTFILE")),
esc_calibrate (0, k_param_esc_calibrate, PSTR("ESC")),
radio_tuning (0, k_param_radio_tuning, PSTR("TUNE")),
frame_orientation (FRAME_ORIENTATION, k_param_frame_orientation, PSTR("FRAME")),
top_bottom_ratio (TOP_BOTTOM_RATIO, k_param_top_bottom_ratio, PSTR("TB_RATIO")),
ch7_option (CH7_OPTION, k_param_ch7_option, PSTR("CH7_OPT")),
log_bitmask (DEFAULT_LOG_BITMASK),
log_last_filenumber (0),
esc_calibrate (0),
radio_tuning (0),
radio_tuning_high (1000),
radio_tuning_low (0),
frame_orientation (FRAME_ORIENTATION),
top_bottom_ratio (TOP_BOTTOM_RATIO),
ch7_option (CH7_OPTION),
auto_slew_rate (AUTO_SLEW_RATE),
#if FRAME_CONFIG == HELI_FRAME
heli_servo_1 (k_param_heli_servo_1, PSTR("HS1_")),
heli_servo_2 (k_param_heli_servo_2, PSTR("HS2_")),
heli_servo_3 (k_param_heli_servo_3, PSTR("HS3_")),
heli_servo_4 (k_param_heli_servo_4, PSTR("HS4_")),
heli_servo1_pos (-60, k_param_heli_servo1_pos, PSTR("SV1_POS_")),
heli_servo2_pos (60, k_param_heli_servo2_pos, PSTR("SV2_POS_")),
heli_servo3_pos (180, k_param_heli_servo3_pos, PSTR("SV3_POS_")),
heli_roll_max (4500, k_param_heli_roll_max, PSTR("ROL_MAX_")),
heli_pitch_max (4500, k_param_heli_pitch_max, PSTR("PIT_MAX_")),
heli_coll_min (1250, k_param_heli_collective_min, PSTR("COL_MIN_")),
heli_coll_max (1750, k_param_heli_collective_max, PSTR("COL_MAX_")),
heli_coll_mid (1500, k_param_heli_collective_mid, PSTR("COL_MID_")),
heli_ext_gyro_enabled (0, k_param_heli_ext_gyro_enabled, PSTR("GYR_ENABLE_")),
heli_ext_gyro_gain (1350, k_param_heli_ext_gyro_gain, PSTR("GYR_GAIN_")),
heli_servo_averaging (0, k_param_heli_servo_averaging, PSTR("SV_AVG")),
heli_servo_manual (0, k_param_heli_servo_manual, PSTR("HSV_MAN")),
heli_phase_angle (0, k_param_heli_phase_angle, PSTR("H_PHANG")),
heli_coll_yaw_effect (0, k_param_heli_coll_yaw_effect, PSTR("H_COLYAW")),
heli_servo_1 (k_param_heli_servo_1),
heli_servo_2 (k_param_heli_servo_2),
heli_servo_3 (k_param_heli_servo_3),
heli_servo_4 (k_param_heli_servo_4),
heli_servo1_pos (-60),
heli_servo2_pos (60),
heli_servo3_pos (180),
heli_roll_max (4500),
heli_pitch_max (4500),
heli_coll_min (1250),
heli_coll_max (1750),
heli_coll_mid (1500),
heli_ext_gyro_enabled (0),
heli_ext_gyro_gain (1350),
heli_servo_averaging (0),
heli_servo_manual (0),
heli_phase_angle (0),
heli_coll_yaw_effect (0),
#endif
// RC channel group key name
camera_pitch_gain (CAM_PITCH_GAIN),
camera_roll_gain (CAM_ROLL_GAIN),
stabilize_d (STABILIZE_D),
acro_p (ACRO_P),
// PID controller initial P initial I initial D initial imax
//-----------------------------------------------------------------------------------------------------
pid_rate_roll (RATE_ROLL_P, RATE_ROLL_I, RATE_ROLL_D, RATE_ROLL_IMAX * 100),
pid_rate_pitch (RATE_PITCH_P, RATE_PITCH_I, RATE_PITCH_D, RATE_PITCH_IMAX * 100),
pid_rate_yaw (RATE_YAW_P, RATE_YAW_I, RATE_YAW_D, RATE_YAW_IMAX * 100),
pid_loiter_rate_lat (LOITER_RATE_P, LOITER_RATE_I, LOITER_RATE_D, LOITER_RATE_IMAX * 100),
pid_loiter_rate_lon (LOITER_RATE_P, LOITER_RATE_I, LOITER_RATE_D, LOITER_RATE_IMAX * 100),
pid_nav_lat (NAV_P, NAV_I, NAV_D, NAV_IMAX * 100),
pid_nav_lon (NAV_P, NAV_I, NAV_D, NAV_IMAX * 100),
pid_throttle (THROTTLE_P, THROTTLE_I, THROTTLE_D, THROTTLE_IMAX),
pid_optflow_roll (OPTFLOW_ROLL_P, OPTFLOW_ROLL_I, OPTFLOW_ROLL_D, OPTFLOW_IMAX * 100),
pid_optflow_pitch (OPTFLOW_PITCH_P, OPTFLOW_PITCH_I, OPTFLOW_PITCH_D, OPTFLOW_IMAX * 100),
// PI controller initial P initial I initial imax
//----------------------------------------------------------------------
rc_1 (k_param_rc_1, PSTR("RC1_")),
rc_2 (k_param_rc_2, PSTR("RC2_")),
rc_3 (k_param_rc_3, PSTR("RC3_")),
rc_4 (k_param_rc_4, PSTR("RC4_")),
rc_5 (k_param_rc_5, PSTR("RC5_")),
rc_6 (k_param_rc_6, PSTR("RC6_")),
rc_7 (k_param_rc_7, PSTR("RC7_")),
rc_8 (k_param_rc_8, PSTR("RC8_")),
rc_camera_pitch (k_param_rc_9, PSTR("CAM_P_")),
rc_camera_roll (k_param_rc_10, PSTR("CAM_R_")),
pi_stabilize_roll (STABILIZE_ROLL_P, STABILIZE_ROLL_I, STABILIZE_ROLL_IMAX * 100),
pi_stabilize_pitch (STABILIZE_PITCH_P, STABILIZE_PITCH_I, STABILIZE_PITCH_IMAX * 100),
pi_stabilize_yaw (STABILIZE_YAW_P, STABILIZE_YAW_I, STABILIZE_YAW_IMAX * 100),
// variable default key name
//-------------------------------------------------------------------------------------------------------------------
camera_pitch_gain (CAM_PITCH_GAIN, k_param_camera_pitch_gain, PSTR("CAM_P_G")),
camera_roll_gain (CAM_ROLL_GAIN, k_param_camera_roll_gain, PSTR("CAM_R_G")),
stablize_d (STABILIZE_D, k_param_stabilize_d, PSTR("STAB_D")),
// PID controller group key name initial P initial I initial D initial imax
//--------------------------------------------------------------------------------------------------------------------------------------------------------------------
pid_rate_roll (k_param_pid_rate_roll, PSTR("RATE_RLL_"), RATE_ROLL_P, RATE_ROLL_I, RATE_ROLL_D, RATE_ROLL_IMAX * 100),
pid_rate_pitch (k_param_pid_rate_pitch, PSTR("RATE_PIT_"), RATE_PITCH_P, RATE_PITCH_I, RATE_PITCH_D, RATE_PITCH_IMAX * 100),
pid_rate_yaw (k_param_pid_rate_yaw, PSTR("RATE_YAW_"), RATE_YAW_P, RATE_YAW_I, RATE_YAW_D, RATE_YAW_IMAX * 100),
pid_nav_lat (k_param_pid_nav_lat, PSTR("NAV_LAT_"), NAV_P, NAV_I, NAV_D, NAV_IMAX * 100),
pid_nav_lon (k_param_pid_nav_lon, PSTR("NAV_LON_"), NAV_P, NAV_I, NAV_D, NAV_IMAX * 100),
pid_throttle (k_param_pid_throttle, PSTR("THR_RATE_"), THROTTLE_P, THROTTLE_I, THROTTLE_D, THROTTLE_IMAX),
pid_optflow_roll (k_param_pid_optflow_roll, PSTR("OF_RLL_"), OPTFLOW_ROLL_P, OPTFLOW_ROLL_I, OPTFLOW_ROLL_D, OPTFLOW_IMAX * 100),
pid_optflow_pitch (k_param_pid_optflow_pitch, PSTR("OF_PIT_"), OPTFLOW_PITCH_P, OPTFLOW_PITCH_I, OPTFLOW_PITCH_D,OPTFLOW_IMAX * 100),
// PI controller group key name initial P initial I initial imax
//--------------------------------------------------------------------------------------------------------------------------------------------------------------------
pi_stabilize_roll (k_param_pi_stabilize_roll, PSTR("STB_RLL_"), STABILIZE_ROLL_P, STABILIZE_ROLL_I, STABILIZE_ROLL_IMAX * 100),
pi_stabilize_pitch (k_param_pi_stabilize_pitch, PSTR("STB_PIT_"), STABILIZE_PITCH_P, STABILIZE_PITCH_I, STABILIZE_PITCH_IMAX * 100),
pi_stabilize_yaw (k_param_pi_stabilize_yaw, PSTR("STB_YAW_"), STABILIZE_YAW_P, STABILIZE_YAW_I, STABILIZE_YAW_IMAX * 100),
pi_alt_hold (k_param_pi_alt_hold, PSTR("THR_ALT_"), ALT_HOLD_P, ALT_HOLD_I, ALT_HOLD_IMAX),
pi_loiter_lat (k_param_pi_loiter_lat, PSTR("HLD_LAT_"), LOITER_P, LOITER_I, LOITER_IMAX * 100),
pi_loiter_lon (k_param_pi_loiter_lon, PSTR("HLD_LON_"), LOITER_P, LOITER_I, LOITER_IMAX * 100),
junk(0) // XXX just so that we can add things without worrying about the trailing comma
pi_alt_hold (ALT_HOLD_P, ALT_HOLD_I, ALT_HOLD_IMAX),
pi_loiter_lat (LOITER_P, LOITER_I, LOITER_IMAX * 100),
pi_loiter_lon (LOITER_P, LOITER_I, LOITER_IMAX * 100)
{
}
};

179
ArduCopter/Parameters.pde Normal file
View File

@ -0,0 +1,179 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
/*
ArduCopter parameter definitions
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.
*/
#define GSCALAR(v, name) { g.v.vtype, name, Parameters::k_param_ ## v, &g.v }
#define GGROUP(v, name, class) { AP_PARAM_GROUP, name, Parameters::k_param_ ## v, &g.v, class::var_info }
#define GOBJECT(v, name, class) { AP_PARAM_GROUP, name, Parameters::k_param_ ## v, &v, class::var_info }
static const AP_Param::Info var_info[] PROGMEM = {
GSCALAR(format_version, "SYSID_SW_MREV"),
GSCALAR(software_type, "SYSID_SW_TYPE"),
GSCALAR(sysid_this_mav, "SYSID_THISMAV"),
GSCALAR(sysid_my_gcs, "SYSID_MYGCS"),
GSCALAR(serial3_baud, "SERIAL3_BAUD"),
GSCALAR(RTL_altitude, "ALT_HOLD_RTL"),
GSCALAR(sonar_enabled, "SONAR_ENABLE"),
GSCALAR(sonar_type, "SONAR_TYPE"),
GSCALAR(battery_monitoring, "BATT_MONITOR"),
GSCALAR(volt_div_ratio, "VOLT_DIVIDER"),
GSCALAR(curr_amp_per_volt, "AMP_PER_VOLT"),
GSCALAR(input_voltage, "INPUT_VOLTS"),
GSCALAR(pack_capacity, "BATT_CAPACITY"),
GSCALAR(compass_enabled, "MAG_ENABLE"),
GSCALAR(optflow_enabled, "FLOW_ENABLE"),
GSCALAR(low_voltage, "LOW_VOLT"),
GSCALAR(super_simple, "SUPER_SIMPLE"),
GSCALAR(rtl_land_enabled, "RTL_LAND"),
GSCALAR(waypoint_mode, "WP_MODE"),
GSCALAR(command_total, "WP_TOTAL"),
GSCALAR(command_index, "WP_INDEX"),
GSCALAR(command_nav_index, "WP_MUST_INDEX"),
GSCALAR(waypoint_radius, "WP_RADIUS"),
GSCALAR(loiter_radius, "WP_LOITER_RAD"),
GSCALAR(waypoint_speed_max, "WP_SPEED_MAX"),
GSCALAR(crosstrack_gain, "XTRK_GAIN_SC"),
GSCALAR(auto_land_timeout, "AUTO_LAND"),
GSCALAR(throttle_min, "THR_MIN"),
GSCALAR(throttle_max, "THR_MAX"),
GSCALAR(throttle_fs_enabled, "THR_FAILSAFE"),
GSCALAR(throttle_fs_action, "THR_FS_ACTION"),
GSCALAR(throttle_fs_value, "THR_FS_VALUE"),
GSCALAR(throttle_cruise, "TRIM_THROTTLE"),
GSCALAR(flight_mode1, "FLTMODE1"),
GSCALAR(flight_mode2, "FLTMODE2"),
GSCALAR(flight_mode3, "FLTMODE3"),
GSCALAR(flight_mode4, "FLTMODE4"),
GSCALAR(flight_mode5, "FLTMODE5"),
GSCALAR(flight_mode6, "FLTMODE6"),
GSCALAR(simple_modes, "SIMPLE"),
GSCALAR(log_bitmask, "LOG_BITMASK"),
GSCALAR(log_last_filenumber, "LOG_LASTFILE"),
GSCALAR(esc_calibrate, "ESC"),
GSCALAR(radio_tuning, "TUNE"),
GSCALAR(radio_tuning_low, "TUNE_LOW"),
GSCALAR(radio_tuning_high, "TUNE_HIGH"),
GSCALAR(frame_orientation, "FRAME"),
GSCALAR(top_bottom_ratio, "TB_RATIO"),
GSCALAR(ch7_option, "CH7_OPT"),
GSCALAR(auto_slew_rate, "AUTO_SLEW"),
#if FRAME_CONFIG == HELI_FRAME
GSCALAR(heli_servo_1, "HS1_"),
GSCALAR(heli_servo_2, "HS2_"),
GSCALAR(heli_servo_3, "HS3_"),
GSCALAR(heli_servo_4, "HS4_"),
GSCALAR(heli_servo1_pos, "SV1_POS_"),
GSCALAR(heli_servo2_pos, "SV2_POS_"),
GSCALAR(heli_servo3_pos, "SV3_POS_"),
GSCALAR(heli_roll_max, "ROL_MAX_"),
GSCALAR(heli_pitch_max, "PIT_MAX_"),
GSCALAR(heli_coll_min, "COL_MIN_"),
GSCALAR(heli_coll_max, "COL_MAX_"),
GSCALAR(heli_coll_mid, "COL_MID_"),
GSCALAR(heli_ext_gyro_enabled, "GYR_ENABLE_"),
GSCALAR(heli_ext_gyro_gain, "GYR_GAIN_"),
GSCALAR(heli_servo_averaging, "SV_AVG"),
GSCALAR(heli_servo_manual, "HSV_MAN"),
GSCALAR(heli_phase_angle, "H_PHANG"),
GSCALAR(heli_coll_yaw_effect, "H_COLYAW"),
#endif
// RC channel
//-----------
GGROUP(rc_1, "RC1_", RC_Channel),
GGROUP(rc_2, "RC2_", RC_Channel),
GGROUP(rc_3, "RC3_", RC_Channel),
GGROUP(rc_4, "RC4_", RC_Channel),
GGROUP(rc_5, "RC5_", RC_Channel),
GGROUP(rc_6, "RC6_", RC_Channel),
GGROUP(rc_7, "RC7_", RC_Channel),
GGROUP(rc_8, "RC8_", RC_Channel),
GGROUP(rc_camera_pitch, "CAM_P_", RC_Channel),
GGROUP(rc_camera_roll, "CAM_R_", RC_Channel),
// variable
//---------
GSCALAR(camera_pitch_gain, "CAM_P_G"),
GSCALAR(camera_roll_gain, "CAM_R_G"),
GSCALAR(stabilize_d, "STAB_D"),
GSCALAR(acro_p, "ACRO_P"),
// PID controller
//---------------
GGROUP(pid_rate_roll, "RATE_RLL_", AC_PID),
GGROUP(pid_rate_pitch, "RATE_PIT_", AC_PID),
GGROUP(pid_rate_yaw, "RATE_YAW_", AC_PID),
GGROUP(pid_loiter_rate_lat, "LOITER_LAT_", AC_PID),
GGROUP(pid_loiter_rate_lon, "LOITER_LON_", AC_PID),
GGROUP(pid_nav_lat, "NAV_LAT_", AC_PID),
GGROUP(pid_nav_lon, "NAV_LON_", AC_PID),
GGROUP(pid_throttle, "THR_RATE_", AC_PID),
GGROUP(pid_optflow_roll, "OF_RLL_", AC_PID),
GGROUP(pid_optflow_pitch, "OF_PIT_", AC_PID),
// PI controller
//--------------
GGROUP(pi_stabilize_roll, "STB_RLL_", APM_PI),
GGROUP(pi_stabilize_pitch, "STB_PIT_", APM_PI),
GGROUP(pi_stabilize_yaw, "STB_YAW_", APM_PI),
GGROUP(pi_alt_hold, "THR_ALT_", APM_PI),
GGROUP(pi_loiter_lat, "HLD_LAT_", APM_PI),
GGROUP(pi_loiter_lon, "HLD_LON_", APM_PI),
// variables not in the g class which contain EEPROM saved variables
GOBJECT(compass, "COMPASS_", Compass),
GOBJECT(gcs0, "SR0_", GCS_MAVLINK),
GOBJECT(gcs3, "SR3_", GCS_MAVLINK)
};
static void load_parameters(void)
{
// setup the AP_Var subsystem for storage to EEPROM
if (!AP_Param::setup(var_info, sizeof(var_info)/sizeof(var_info[0]), WP_START_BYTE)) {
// this can only happen on startup, and its a definate coding
// error. Best not to continue so the programmer catches it
while (1) {
Serial.println_P(PSTR("ERROR: Failed to setup AP_Param"));
delay(1000);
}
}
if (!g.format_version.load() ||
g.format_version != Parameters::k_format_version) {
// erase all parameters
Serial.printf_P(PSTR("Firmware change: erasing EEPROM...\n"));
AP_Param::erase_all();
// save the current format version
g.format_version.set_and_save(Parameters::k_format_version);
default_dead_zones();
Serial.println_P(PSTR("done."));
} else {
unsigned long before = micros();
// Load all auto-loaded EEPROM variables
AP_Param::load_all();
Serial.printf_P(PSTR("load_all took %luus\n"), micros() - before);
}
}

57
ArduCopter/config.h
View File

@ -188,6 +188,13 @@
# define CONFIG_SONAR ENABLED
#endif
//////////////////////////////////////////////////////////////////////////////
// Channel Config (custom MOT channel mappings)
//
#ifndef CONFIG_CHANNELS
# define CONFIG_CHANNELS CHANNEL_CONFIG_DEFAULT
#endif
//////////////////////////////////////////////////////////////////////////////
// Acrobatics
@ -503,6 +510,12 @@
# define SUPER_SIMPLE DISABLED
#endif
// RTL Mode
#ifndef RTL_AUTO_LAND
# define RTL_AUTO_LAND ENABLED
#endif
// LOITER Mode
#ifndef OF_LOITER_YAW
# define OF_LOITER_YAW YAW_HOLD
@ -542,10 +555,17 @@
#ifndef STABILIZE_D
# define STABILIZE_D .12
# define STABILIZE_D .06
#endif
// Jasons default values that are good for smaller payload motors.
#ifndef ACRO_P
# define ACRO_P 4.5
#endif
// Good for smaller payload motors.
#ifndef STABILIZE_ROLL_P
# define STABILIZE_ROLL_P 4.5
#endif
@ -587,10 +607,10 @@
# define RATE_ROLL_I 0.18
#endif
#ifndef RATE_ROLL_D
# define RATE_ROLL_D 0.0
# define RATE_ROLL_D 0.0025
#endif
#ifndef RATE_ROLL_IMAX
# define RATE_ROLL_IMAX 15 // degrees
# define RATE_ROLL_IMAX 5 // degrees
#endif
#ifndef RATE_PITCH_P
@ -600,10 +620,10 @@
# define RATE_PITCH_I 0.18
#endif
#ifndef RATE_PITCH_D
# define RATE_PITCH_D 0.0 // 0.002
# define RATE_PITCH_D 0.0025
#endif
#ifndef RATE_PITCH_IMAX
# define RATE_PITCH_IMAX 15 // degrees
# define RATE_PITCH_IMAX 5 // degrees
#endif
#ifndef RATE_YAW_P
@ -633,6 +653,22 @@
# define LOITER_IMAX 30 // degrees
#endif
//////////////////////////////////////////////////////////////////////////////
// Loiter Navigation control gains
//
#ifndef LOITER_RATE_P
# define LOITER_RATE_P 3.0 //
#endif
#ifndef LOITER_RATE_I
# define LOITER_RATE_I 0.1 // Wind control
#endif
#ifndef LOITER_RATE_D
# define LOITER_RATE_D 0.00 //
#endif
#ifndef LOITER_RATE_IMAX
# define LOITER_RATE_IMAX 30 // degrees
#endif
//////////////////////////////////////////////////////////////////////////////
// WP Navigation control gains
//
@ -640,7 +676,7 @@
# define NAV_P 3.0 //
#endif
#ifndef NAV_I
# define NAV_I 0.4 // Wind control
# define NAV_I 0.1 // Wind control
#endif
#ifndef NAV_D
# define NAV_D 0.00 //
@ -649,6 +685,13 @@
# define NAV_IMAX 30 // degrees
#endif
#ifndef AUTO_SLEW_RATE
# define AUTO_SLEW_RATE 30 // degrees
#endif
#ifndef WAYPOINT_SPEED_MAX
# define WAYPOINT_SPEED_MAX 600 // 6m/s error = 13mph
#endif

41
ArduCopter/config_channels.h
View File

@ -16,31 +16,34 @@
//////////////////////////////////////////////////////////////////////////////
//
#include "config.h"
#include "config.h" // Parent Config File
#include "APM_Config.h" // User Overrides
//
//
// Output CH mapping for ArduCopter motor channels
//
//
#if CONFIG_APM_HARDWARE == APM_HARDWARE_APM2
# define MOT_1 CH_1
# define MOT_2 CH_2
# define MOT_3 CH_3
# define MOT_4 CH_4
# define MOT_5 CH_5
# define MOT_6 CH_6
# define MOT_7 CH_7
# define MOT_8 CH_8
#elif CONFIG_APM_HARDWARE == APM_HARDWARE_APM1
# define MOT_1 CH_1
# define MOT_2 CH_2
# define MOT_3 CH_3
# define MOT_4 CH_4
# define MOT_5 CH_7
# define MOT_6 CH_8
# define MOT_7 CH_10
# define MOT_8 CH_11
#if CONFIG_CHANNELS == CHANNEL_CONFIG_DEFAULT
# if CONFIG_APM_HARDWARE == APM_HARDWARE_APM2
# define MOT_1 CH_1
# define MOT_2 CH_2
# define MOT_3 CH_3
# define MOT_4 CH_4
# define MOT_5 CH_5
# define MOT_6 CH_6
# define MOT_7 CH_7
# define MOT_8 CH_8
# elif CONFIG_APM_HARDWARE == APM_HARDWARE_APM1
# define MOT_1 CH_1
# define MOT_2 CH_2
# define MOT_3 CH_3
# define MOT_4 CH_4
# define MOT_5 CH_7
# define MOT_6 CH_8
# define MOT_7 CH_10
# define MOT_8 CH_11
# endif
#endif
//

7
ArduCopter/defines.h
View File

@ -345,7 +345,7 @@ enum gcs_severity {
// EEPROM addresses
#define EEPROM_MAX_ADDR 4096
// parameters get the first 1KiB of EEPROM, remainder is for waypoints
// parameters get the first 1024 bytes of EEPROM, remainder is for waypoints
#define WP_START_BYTE 0x400 // where in memory home WP is stored + all other WP
#define WP_SIZE 15
@ -369,4 +369,9 @@ enum gcs_severity {
#define LOGGING_SIMPLE 1
#define LOGGING_VERBOSE 2
// Channel Config selection
#define CHANNEL_CONFIG_DEFAULT 1
#define CHANNEL_CONFIG_CUSTOM 2
#endif // _DEFINES_H

102
ArduCopter/motors_hexa.pde
View File

@ -158,6 +158,8 @@ static void output_motors_disarmed()
static void output_motor_test()
{
motors_output_enable();
motor_out[MOT_1] = g.rc_3.radio_min;
motor_out[MOT_2] = g.rc_3.radio_min;
motor_out[MOT_3] = g.rc_3.radio_min;
@ -165,48 +167,53 @@ static void output_motor_test()
motor_out[MOT_5] = g.rc_3.radio_min;
motor_out[MOT_6] = g.rc_3.radio_min;
bool right = (g.rc_1.control_in > 3000);
bool left = (g.rc_1.control_in < -3000);
bool front = (g.rc_2.control_in < -3000);
bool back = (g.rc_2.control_in > 3000);
if(g.frame_orientation == X_FRAME){
// 31
// 24
if(g.rc_1.control_in > 3000){ // right
motor_out[MOT_1] += 100;
if(right && !(front || back))
motor_out[MOT_1] += 150; // Right
if(left && !(front || back))
motor_out[MOT_2] += 150; // Left
if(back){
if(left)
motor_out[MOT_6] += 150;
if(right)
motor_out[MOT_4] += 150;
}
if(g.rc_1.control_in < -3000){ // left
motor_out[MOT_2] += 100;
if(front){
if(left)
motor_out[MOT_3] += 150;
if(right)
motor_out[MOT_5] += 150;
}
if(g.rc_2.control_in > 3000){ // back
motor_out[MOT_6] += 100;
motor_out[MOT_4] += 100;
} else { /* PLUS_FRAME */
if(front && !(left || right))
motor_out[MOT_1] += 150;
if(back && !(left || right))
motor_out[MOT_2] += 150;
if(left){
if(front)
motor_out[MOT_5] += 150;
if(back)
motor_out[MOT_3] += 150;
}
if(g.rc_2.control_in < -3000){ // front
motor_out[MOT_5] += 100;
motor_out[MOT_3] += 100;
}
}else{
// 3
// 2 1
// 4
if(g.rc_1.control_in > 3000){ // right
motor_out[MOT_4] += 100;
motor_out[MOT_6] += 100;
}
if(g.rc_1.control_in < -3000){ // left
motor_out[MOT_5] += 100;
motor_out[MOT_3] += 100;
}
if(g.rc_2.control_in > 3000){ // back
motor_out[MOT_2] += 100;
}
if(g.rc_2.control_in < -3000){ // front
motor_out[MOT_1] += 100;
if(right){
if(front)
motor_out[MOT_4] += 150;
if(back)
motor_out[MOT_6] += 150;
}
}
@ -219,31 +226,4 @@ static void output_motor_test()
APM_RC.OutputCh(MOT_6, motor_out[MOT_6]);
}
/*
APM_RC.OutputCh(MOT_2, g.rc_3.radio_min);
APM_RC.OutputCh(MOT_3, g.rc_3.radio_min + 100);
delay(1000);
APM_RC.OutputCh(MOT_3, g.rc_3.radio_min);
APM_RC.OutputCh(MOT_5, g.rc_3.radio_min + 100);
delay(1000);
APM_RC.OutputCh(MOT_5, g.rc_3.radio_min);
APM_RC.OutputCh(MOT_1, g.rc_3.radio_min + 100);
delay(1000);
APM_RC.OutputCh(MOT_1, g.rc_3.radio_min);
APM_RC.OutputCh(MOT_4, g.rc_3.radio_min + 100);
delay(1000);
APM_RC.OutputCh(MOT_4, g.rc_3.radio_min);
APM_RC.OutputCh(MOT_6, g.rc_3.radio_min + 100);
delay(1000);
APM_RC.OutputCh(MOT_6, g.rc_3.radio_min);
APM_RC.OutputCh(MOT_2, g.rc_3.radio_min + 100);
delay(1000);
}
*/
#endif

245
ArduCopter/motors_octa.pde
View File

@ -5,21 +5,21 @@
static void init_motors_out()
{
#if INSTANT_PWM == 0
APM_RC.SetFastOutputChannels( _BV(MOT_1) | _BV(MOT_2) | _BV(MOT_3) | _BV(MOT_4)
| _BV(MOT_5) | _BV(MOT_6) | _BV(MOT_7) | _BV(MOT_8) );
APM_RC.SetFastOutputChannels(_BV(MOT_1) | _BV(MOT_2) | _BV(MOT_3) | _BV(MOT_4)
| _BV(MOT_5) | _BV(MOT_6) | _BV(MOT_7) | _BV(MOT_8));
#endif
}
static void motors_output_enable()
{
APM_RC.enable_out(MOT_1);
APM_RC.enable_out(MOT_2);
APM_RC.enable_out(MOT_3);
APM_RC.enable_out(MOT_4);
APM_RC.enable_out(MOT_5);
APM_RC.enable_out(MOT_6);
APM_RC.enable_out(MOT_7);
APM_RC.enable_out(MOT_8);
APM_RC.enable_out(MOT_1);
APM_RC.enable_out(MOT_2);
APM_RC.enable_out(MOT_3);
APM_RC.enable_out(MOT_4);
APM_RC.enable_out(MOT_5);
APM_RC.enable_out(MOT_6);
APM_RC.enable_out(MOT_7);
APM_RC.enable_out(MOT_8);
}
static void output_motors_armed()
@ -40,16 +40,16 @@ static void output_motors_armed()
g.rc_4.calc_pwm();
if(g.frame_orientation == X_FRAME){
roll_out = (float)g.rc_1.pwm_out * 0.4;
pitch_out = (float)g.rc_2.pwm_out * 0.4;
roll_out = (float)g.rc_1.pwm_out * 0.4;
pitch_out = (float)g.rc_2.pwm_out * 0.4;
//Front side
motor_out[MOT_1] = g.rc_3.radio_out + g.rc_2.pwm_out - roll_out; // CW FRONT RIGHT
motor_out[MOT_1] = g.rc_3.radio_out + g.rc_2.pwm_out - roll_out; // CW FRONT RIGHT
motor_out[MOT_5] = g.rc_3.radio_out + g.rc_2.pwm_out + roll_out; // CCW FRONT LEFT
//Back side
motor_out[MOT_2] = g.rc_3.radio_out - g.rc_2.pwm_out + roll_out; // CW BACK LEFT
motor_out[MOT_4] = g.rc_3.radio_out - g.rc_2.pwm_out - roll_out; // CCW BACK RIGHT
motor_out[MOT_2] = g.rc_3.radio_out - g.rc_2.pwm_out + roll_out; // CW BACK LEFT
motor_out[MOT_4] = g.rc_3.radio_out - g.rc_2.pwm_out - roll_out; // CCW BACK RIGHT
//Left side
motor_out[MOT_7] = g.rc_3.radio_out + g.rc_1.pwm_out + pitch_out; // CW LEFT FRONT
@ -57,14 +57,14 @@ static void output_motors_armed()
//Right side
motor_out[MOT_8] = g.rc_3.radio_out - g.rc_1.pwm_out - pitch_out; // CW RIGHT BACK
motor_out[MOT_3] = g.rc_3.radio_out - g.rc_1.pwm_out + pitch_out; // CCW RIGHT FRONT
motor_out[MOT_3] = g.rc_3.radio_out - g.rc_1.pwm_out + pitch_out; // CCW RIGHT FRONT
}else if(g.frame_orientation == PLUS_FRAME){
roll_out = (float)g.rc_1.pwm_out * 0.71;
pitch_out = (float)g.rc_2.pwm_out * 0.71;
roll_out = (float)g.rc_1.pwm_out * 0.71;
pitch_out = (float)g.rc_2.pwm_out * 0.71;
//Front side
motor_out[MOT_1] = g.rc_3.radio_out + g.rc_2.pwm_out; // CW FRONT
motor_out[MOT_1] = g.rc_3.radio_out + g.rc_2.pwm_out; // CW FRONT
motor_out[MOT_3] = g.rc_3.radio_out - roll_out + pitch_out; // CCW FRONT RIGHT
motor_out[MOT_5] = g.rc_3.radio_out + roll_out + pitch_out; // CCW FRONT LEFT
@ -75,9 +75,9 @@ static void output_motors_armed()
motor_out[MOT_8] = g.rc_3.radio_out - g.rc_1.pwm_out; // CW RIGHT
//Back side
motor_out[MOT_2] = g.rc_3.radio_out - g.rc_2.pwm_out; // CW BACK
motor_out[MOT_4] = g.rc_3.radio_out - roll_out - pitch_out; // CCW BACK RIGHT
motor_out[MOT_6] = g.rc_3.radio_out + roll_out - pitch_out; // CCW BACK LEFT
motor_out[MOT_2] = g.rc_3.radio_out - g.rc_2.pwm_out; // CW BACK
motor_out[MOT_4] = g.rc_3.radio_out - roll_out - pitch_out; // CCW BACK RIGHT
motor_out[MOT_6] = g.rc_3.radio_out + roll_out - pitch_out; // CCW BACK LEFT
}else if(g.frame_orientation == V_FRAME){
@ -95,63 +95,63 @@ static void output_motors_armed()
pitch_out4 = (float)g.rc_2.pwm_out * 0.98;
//Front side
motor_out[MOT_7] = g.rc_3.radio_out + g.rc_2.pwm_out - roll_out; // CW FRONT RIGHT
motor_out[MOT_7] = g.rc_3.radio_out + g.rc_2.pwm_out - roll_out; // CW FRONT RIGHT
motor_out[MOT_5] = g.rc_3.radio_out + g.rc_2.pwm_out + roll_out; // CCW FRONT LEFT
//Left side
motor_out[MOT_1] = g.rc_3.radio_out + g.rc_1.pwm_out + pitch_out2; // CW LEFT FRONT
motor_out[MOT_1] = g.rc_3.radio_out + g.rc_1.pwm_out + pitch_out2; // CW LEFT FRONT
motor_out[MOT_3] = g.rc_3.radio_out + g.rc_1.pwm_out - pitch_out3; // CCW LEFT BACK
//Right side
motor_out[MOT_2] = g.rc_3.radio_out - g.rc_1.pwm_out - pitch_out3; // CW RIGHT BACK
motor_out[MOT_2] = g.rc_3.radio_out - g.rc_1.pwm_out - pitch_out3; // CW RIGHT BACK
motor_out[MOT_6] = g.rc_3.radio_out - g.rc_1.pwm_out + pitch_out2; // CCW RIGHT FRONT
//Back side
motor_out[MOT_8] = g.rc_3.radio_out - g.rc_2.pwm_out + roll_out4; // CW BACK LEFT
motor_out[MOT_4] = g.rc_3.radio_out - g.rc_2.pwm_out - roll_out4; // CCW BACK RIGHT
motor_out[MOT_8] = g.rc_3.radio_out - g.rc_2.pwm_out + roll_out4; // CW BACK LEFT
motor_out[MOT_4] = g.rc_3.radio_out - g.rc_2.pwm_out - roll_out4; // CCW BACK RIGHT
}
// Yaw
motor_out[MOT_3] += g.rc_4.pwm_out; // CCW
motor_out[MOT_4] += g.rc_4.pwm_out; // CCW
motor_out[MOT_3] += g.rc_4.pwm_out; // CCW
motor_out[MOT_4] += g.rc_4.pwm_out; // CCW
motor_out[MOT_5] += g.rc_4.pwm_out; // CCW
motor_out[MOT_6] += g.rc_4.pwm_out; // CCW
motor_out[MOT_1] -= g.rc_4.pwm_out; // CW
motor_out[MOT_2] -= g.rc_4.pwm_out; // CW
motor_out[MOT_1] -= g.rc_4.pwm_out; // CW
motor_out[MOT_2] -= g.rc_4.pwm_out; // CW
motor_out[MOT_7] -= g.rc_4.pwm_out; // CW
motor_out[MOT_8] -= g.rc_4.pwm_out; // CW
// TODO add stability patch
motor_out[MOT_1] = min(motor_out[MOT_1], out_max);
motor_out[MOT_2] = min(motor_out[MOT_2], out_max);
motor_out[MOT_3] = min(motor_out[MOT_3], out_max);
motor_out[MOT_4] = min(motor_out[MOT_4], out_max);
motor_out[MOT_5] = min(motor_out[MOT_5], out_max);
motor_out[MOT_6] = min(motor_out[MOT_6], out_max);
motor_out[MOT_7] = min(motor_out[MOT_7], out_max);
motor_out[MOT_8] = min(motor_out[MOT_8], out_max);
motor_out[MOT_1] = min(motor_out[MOT_1], out_max);
motor_out[MOT_2] = min(motor_out[MOT_2], out_max);
motor_out[MOT_3] = min(motor_out[MOT_3], out_max);
motor_out[MOT_4] = min(motor_out[MOT_4], out_max);
motor_out[MOT_5] = min(motor_out[MOT_5], out_max);
motor_out[MOT_6] = min(motor_out[MOT_6], out_max);
motor_out[MOT_7] = min(motor_out[MOT_7], out_max);
motor_out[MOT_8] = min(motor_out[MOT_8], out_max);
// limit output so motors don't stop
motor_out[MOT_1] = max(motor_out[MOT_1], out_min);
motor_out[MOT_2] = max(motor_out[MOT_2], out_min);
motor_out[MOT_3] = max(motor_out[MOT_3], out_min);
motor_out[MOT_4] = max(motor_out[MOT_4], out_min);
motor_out[MOT_5] = max(motor_out[MOT_5], out_min);
motor_out[MOT_6] = max(motor_out[MOT_6], out_min);
motor_out[MOT_7] = max(motor_out[MOT_7], out_min);
motor_out[MOT_8] = max(motor_out[MOT_8], out_min);
motor_out[MOT_1] = max(motor_out[MOT_1], out_min);
motor_out[MOT_2] = max(motor_out[MOT_2], out_min);
motor_out[MOT_3] = max(motor_out[MOT_3], out_min);
motor_out[MOT_4] = max(motor_out[MOT_4], out_min);
motor_out[MOT_5] = max(motor_out[MOT_5], out_min);
motor_out[MOT_6] = max(motor_out[MOT_6], out_min);
motor_out[MOT_7] = max(motor_out[MOT_7], out_min);
motor_out[MOT_8] = max(motor_out[MOT_8], out_min);
#if CUT_MOTORS == ENABLED
// if we are not sending a throttle output, we cut the motors
if(g.rc_3.servo_out == 0){
motor_out[MOT_1] = g.rc_3.radio_min;
motor_out[MOT_2] = g.rc_3.radio_min;
motor_out[MOT_3] = g.rc_3.radio_min;
motor_out[MOT_1] = g.rc_3.radio_min;
motor_out[MOT_2] = g.rc_3.radio_min;
motor_out[MOT_3] = g.rc_3.radio_min;
motor_out[MOT_4] = g.rc_3.radio_min;
motor_out[MOT_5] = g.rc_3.radio_min;
motor_out[MOT_6] = g.rc_3.radio_min;
@ -162,8 +162,8 @@ static void output_motors_armed()
// this filter slows the acceleration of motors vs the deceleration
// Idea by Denny Rowland to help with his Yaw issue
for(int8_t m = 0; m <= 8; m++ ) {
int c = ch_of_mot(m);
for(int8_t m = 0; m <= 8; m++){
int c = ch_of_mot(m);
if(motor_filtered[c] < motor_out[c]){
motor_filtered[c] = (motor_out[c] + motor_filtered[c]) / 2;
}else{
@ -198,7 +198,7 @@ static void output_motors_disarmed()
}
// fill the motor_out[] array for HIL use
for (unsigned char i = 0; i < 11; i++) {
for (unsigned char i = 0; i < 11; i++){
motor_out[i] = g.rc_3.radio_min;
}
@ -216,75 +216,110 @@ static void output_motors_disarmed()
static void output_motor_test()
{
if( g.frame_orientation == X_FRAME || g.frame_orientation == PLUS_FRAME )
{
APM_RC.OutputCh(MOT_5, g.rc_3.radio_min);
APM_RC.OutputCh(MOT_1, g.rc_3.radio_min + 100);
delay(1000);
APM_RC.OutputCh(MOT_1, g.rc_3.radio_min);
APM_RC.OutputCh(MOT_3, g.rc_3.radio_min + 100);
delay(1000);
motor_out[MOT_1] = g.rc_3.radio_min;
motor_out[MOT_2] = g.rc_3.radio_min;
motor_out[MOT_3] = g.rc_3.radio_min;
motor_out[MOT_4] = g.rc_3.radio_min;
motor_out[MOT_5] = g.rc_3.radio_min;
motor_out[MOT_6] = g.rc_3.radio_min;
motor_out[MOT_7] = g.rc_3.radio_min;
motor_out[MOT_8] = g.rc_3.radio_min;
APM_RC.OutputCh(MOT_3, g.rc_3.radio_min);
APM_RC.OutputCh(MOT_8, g.rc_3.radio_min + 100);
delay(1000);
APM_RC.OutputCh(MOT_8, g.rc_3.radio_min);
APM_RC.OutputCh(MOT_4, g.rc_3.radio_min + 100);
delay(1000);
if(g.frame_orientation == X_FRAME || g.frame_orientation == PLUS_FRAME){
APM_RC.OutputCh(MOT_5, g.rc_3.radio_min);
delay(4000);
APM_RC.OutputCh(MOT_1, g.rc_3.radio_min + 100);
delay(300);
APM_RC.OutputCh(MOT_4, g.rc_3.radio_min);
APM_RC.OutputCh(MOT_2, g.rc_3.radio_min + 100);
delay(1000);
APM_RC.OutputCh(MOT_1, g.rc_3.radio_min);
delay(2000);
APM_RC.OutputCh(MOT_3, g.rc_3.radio_min + 100);
delay(300);
APM_RC.OutputCh(MOT_2, g.rc_3.radio_min);
APM_RC.OutputCh(MOT_6, g.rc_3.radio_min + 100);
delay(1000);
APM_RC.OutputCh(MOT_3, g.rc_3.radio_min);
delay(2000);
APM_RC.OutputCh(MOT_8, g.rc_3.radio_min + 100);
delay(300);
APM_RC.OutputCh(MOT_6, g.rc_3.radio_min);
APM_RC.OutputCh(MOT_7, g.rc_3.radio_min + 100);
delay(1000);
APM_RC.OutputCh(MOT_8, g.rc_3.radio_min);
delay(2000);
APM_RC.OutputCh(MOT_4, g.rc_3.radio_min + 100);
delay(300);
APM_RC.OutputCh(MOT_7, g.rc_3.radio_min);
APM_RC.OutputCh(MOT_5, g.rc_3.radio_min + 100);
delay(1000);
APM_RC.OutputCh(MOT_4, g.rc_3.radio_min);
delay(2000);
APM_RC.OutputCh(MOT_2, g.rc_3.radio_min + 100);
delay(300);
APM_RC.OutputCh(MOT_2, g.rc_3.radio_min);
delay(2000);
APM_RC.OutputCh(MOT_6, g.rc_3.radio_min + 100);
delay(300);
APM_RC.OutputCh(MOT_6, g.rc_3.radio_min);
delay(2000);
APM_RC.OutputCh(MOT_7, g.rc_3.radio_min + 100);
delay(300);
APM_RC.OutputCh(MOT_7, g.rc_3.radio_min);
delay(2000);
APM_RC.OutputCh(MOT_5, g.rc_3.radio_min + 100);
delay(300);
}
if( g.frame_orientation == V_FRAME )
{
APM_RC.OutputCh(MOT_5, g.rc_3.radio_min);
APM_RC.OutputCh(MOT_7, g.rc_3.radio_min + 100);
delay(1000);
if(g.frame_orientation == V_FRAME){
APM_RC.OutputCh(MOT_5, g.rc_3.radio_min);
delay(4000);
APM_RC.OutputCh(MOT_7, g.rc_3.radio_min + 100);
delay(300);
APM_RC.OutputCh(MOT_7, g.rc_3.radio_min);
APM_RC.OutputCh(MOT_6, g.rc_3.radio_min + 100);
delay(1000);
APM_RC.OutputCh(MOT_7, g.rc_3.radio_min);
delay(2000);
APM_RC.OutputCh(MOT_6, g.rc_3.radio_min + 100);
delay(300);
APM_RC.OutputCh(MOT_6, g.rc_3.radio_min);
APM_RC.OutputCh(MOT_2, g.rc_3.radio_min + 100);
delay(1000);
APM_RC.OutputCh(MOT_6, g.rc_3.radio_min);
delay(2000);
APM_RC.OutputCh(MOT_2, g.rc_3.radio_min + 100);
delay(300);
APM_RC.OutputCh(MOT_2, g.rc_3.radio_min);
APM_RC.OutputCh(MOT_4, g.rc_3.radio_min + 100);
delay(1000);
APM_RC.OutputCh(MOT_2, g.rc_3.radio_min);
delay(2000);
APM_RC.OutputCh(MOT_4, g.rc_3.radio_min + 100);
delay(300);
APM_RC.OutputCh(MOT_4, g.rc_3.radio_min);
APM_RC.OutputCh(MOT_8, g.rc_3.radio_min + 100);
delay(1000);
APM_RC.OutputCh(MOT_4, g.rc_3.radio_min);
delay(2000);
APM_RC.OutputCh(MOT_8, g.rc_3.radio_min + 100);
delay(300);
APM_RC.OutputCh(MOT_8, g.rc_3.radio_min);
APM_RC.OutputCh(MOT_3, g.rc_3.radio_min + 100);
delay(1000);
APM_RC.OutputCh(MOT_8, g.rc_3.radio_min);
delay(2000);
APM_RC.OutputCh(MOT_3, g.rc_3.radio_min + 100);
delay(300);
APM_RC.OutputCh(MOT_3, g.rc_3.radio_min);
APM_RC.OutputCh(MOT_1, g.rc_3.radio_min + 100);
delay(1000);
APM_RC.OutputCh(MOT_3, g.rc_3.radio_min);
delay(2000);
APM_RC.OutputCh(MOT_1, g.rc_3.radio_min + 100);
delay(300);
APM_RC.OutputCh(MOT_1, g.rc_3.radio_min);
APM_RC.OutputCh(MOT_5, g.rc_3.radio_min + 100);
delay(1000);
APM_RC.OutputCh(MOT_1, g.rc_3.radio_min);
delay(2000);
APM_RC.OutputCh(MOT_5, g.rc_3.radio_min + 100);
delay(300);
}
APM_RC.OutputCh(MOT_1, motor_out[MOT_1]);
APM_RC.OutputCh(MOT_2, motor_out[MOT_2]);
APM_RC.OutputCh(MOT_3, motor_out[MOT_3]);
APM_RC.OutputCh(MOT_4, motor_out[MOT_4]);
APM_RC.OutputCh(MOT_5, motor_out[MOT_5]);
APM_RC.OutputCh(MOT_6, motor_out[MOT_6]);
APM_RC.OutputCh(MOT_7, motor_out[MOT_7]);
APM_RC.OutputCh(MOT_8, motor_out[MOT_8]);
}
#endif

190
ArduCopter/motors_octa_quad.pde
View File

@ -5,21 +5,21 @@
static void init_motors_out()
{
#if INSTANT_PWM == 0
APM_RC.SetFastOutputChannels( _BV(MOT_1) | _BV(MOT_2) | _BV(MOT_3) | _BV(MOT_4)
| _BV(MOT_5) | _BV(MOT_6) | _BV(MOT_7) | _BV(MOT_8) );
APM_RC.SetFastOutputChannels(_BV(MOT_1) | _BV(MOT_2) | _BV(MOT_3) | _BV(MOT_4)
| _BV(MOT_5) | _BV(MOT_6) | _BV(MOT_7) | _BV(MOT_8));
#endif
}
static void motors_output_enable()
{
APM_RC.enable_out(MOT_1);
APM_RC.enable_out(MOT_2);
APM_RC.enable_out(MOT_3);
APM_RC.enable_out(MOT_4);
APM_RC.enable_out(MOT_5);
APM_RC.enable_out(MOT_6);
APM_RC.enable_out(MOT_7);
APM_RC.enable_out(MOT_8);
APM_RC.enable_out(MOT_1);
APM_RC.enable_out(MOT_2);
APM_RC.enable_out(MOT_3);
APM_RC.enable_out(MOT_4);
APM_RC.enable_out(MOT_5);
APM_RC.enable_out(MOT_6);
APM_RC.enable_out(MOT_7);
APM_RC.enable_out(MOT_8);
}
static void output_motors_armed()
@ -47,63 +47,62 @@ static void output_motors_armed()
motor_out[MOT_2] = ((g.rc_3.radio_out * g.top_bottom_ratio) + roll_out + pitch_out); // APM2 OUT2 APM1 OUT2 FRONT LEFT CW TOP
motor_out[MOT_3] = ((g.rc_3.radio_out * g.top_bottom_ratio) + roll_out - pitch_out); // APM2 OUT3 APM1 OUT3 BACK LEFT CCW TOP
motor_out[MOT_4] = ((g.rc_3.radio_out * g.top_bottom_ratio) - roll_out - pitch_out); // APM2 OUT4 APM1 OUT4 BACK RIGHT CW TOP
motor_out[MOT_5] = g.rc_3.radio_out + roll_out + pitch_out; // APM2 OUT5 APM1 OUT7 FRONT LEFT CCW BOTTOM
motor_out[MOT_6] = g.rc_3.radio_out - roll_out + pitch_out; // APM2 OUT6 APM1 OUT8 FRONT RIGHT CW BOTTOM
motor_out[MOT_7] = g.rc_3.radio_out - roll_out - pitch_out; // APM2 OUT7 APM1 OUT10 BACK RIGHT CCW BOTTOM
motor_out[MOT_8] = g.rc_3.radio_out + roll_out - pitch_out; // APM2 OUT8 APM1 OUT11 BACK LEFT CW BOTTOM
motor_out[MOT_5] = g.rc_3.radio_out + roll_out + pitch_out; // APM2 OUT5 APM1 OUT7 FRONT LEFT CCW BOTTOM
motor_out[MOT_6] = g.rc_3.radio_out - roll_out + pitch_out; // APM2 OUT6 APM1 OUT8 FRONT RIGHT CW BOTTOM
motor_out[MOT_7] = g.rc_3.radio_out - roll_out - pitch_out; // APM2 OUT7 APM1 OUT10 BACK RIGHT CCW BOTTOM
motor_out[MOT_8] = g.rc_3.radio_out + roll_out - pitch_out; // APM2 OUT8 APM1 OUT11 BACK LEFT CW BOTTOM
}else{
roll_out = g.rc_1.pwm_out;
pitch_out = g.rc_2.pwm_out;
roll_out = g.rc_1.pwm_out;
pitch_out = g.rc_2.pwm_out;
motor_out[MOT_1] = (g.rc_3.radio_out * g.top_bottom_ratio) + pitch_out; //APM2 OUT1 APM1 OUT1 FRONT CCW TOP
motor_out[MOT_2] = (g.rc_3.radio_out * g.top_bottom_ratio) + roll_out; //APM2 OUT2 APM1 OUT2 LEFT CW TOP
motor_out[MOT_3] = (g.rc_3.radio_out * g.top_bottom_ratio) - pitch_out; //APM2 OUT3 APM1 OUT3 BACK CCW TOP
motor_out[MOT_4] = (g.rc_3.radio_out * g.top_bottom_ratio) - roll_out; //APM2 OUT4 APM1 OUT4 RIGHT CW TOP
motor_out[MOT_5] = g.rc_3.radio_out + roll_out; //APM2 OUT5 APM1 OUT7 LEFT CCW BOTTOM
motor_out[MOT_6] = g.rc_3.radio_out + pitch_out; //APM2 OUT6 APM1 OUT8 FRONT CW BOTTOM
motor_out[MOT_7] = g.rc_3.radio_out - roll_out; //APM2 OUT7 APM1 OUT10 RIGHT CCW BOTTOM
motor_out[MOT_8] = g.rc_3.radio_out - pitch_out; //APM2 OUT8 APM1 OUT11 BACK CW BOTTOM
motor_out[MOT_1] = (g.rc_3.radio_out * g.top_bottom_ratio) + pitch_out; // APM2 OUT1 APM1 OUT1 FRONT CCW TOP
motor_out[MOT_2] = (g.rc_3.radio_out * g.top_bottom_ratio) + roll_out; // APM2 OUT2 APM1 OUT2 LEFT CW TOP
motor_out[MOT_3] = (g.rc_3.radio_out * g.top_bottom_ratio) - pitch_out; // APM2 OUT3 APM1 OUT3 BACK CCW TOP
motor_out[MOT_4] = (g.rc_3.radio_out * g.top_bottom_ratio) - roll_out; // APM2 OUT4 APM1 OUT4 RIGHT CW TOP
motor_out[MOT_5] = g.rc_3.radio_out + roll_out; // APM2 OUT5 APM1 OUT7 LEFT CCW BOTTOM
motor_out[MOT_6] = g.rc_3.radio_out + pitch_out; // APM2 OUT6 APM1 OUT8 FRONT CW BOTTOM
motor_out[MOT_7] = g.rc_3.radio_out - roll_out; // APM2 OUT7 APM1 OUT10 RIGHT CCW BOTTOM
motor_out[MOT_8] = g.rc_3.radio_out - pitch_out; // APM2 OUT8 APM1 OUT11 BACK CW BOTTOM
// Yaw
motor_out[MOT_1] += g.rc_4.pwm_out; // CCW
motor_out[MOT_3] += g.rc_4.pwm_out; // CCW
motor_out[MOT_5] += g.rc_4.pwm_out; // CCW
motor_out[MOT_7] += g.rc_4.pwm_out; // CCW
// Yaw
motor_out[MOT_1] += g.rc_4.pwm_out; // CCW
motor_out[MOT_3] += g.rc_4.pwm_out; // CCW
motor_out[MOT_5] += g.rc_4.pwm_out; // CCW
motor_out[MOT_7] += g.rc_4.pwm_out; // CCW
motor_out[MOT_2] -= g.rc_4.pwm_out; // CW
motor_out[MOT_4] -= g.rc_4.pwm_out; // CW
motor_out[MOT_6] -= g.rc_4.pwm_out; // CW
motor_out[MOT_8] -= g.rc_4.pwm_out; // CW
motor_out[MOT_2] -= g.rc_4.pwm_out; // CW
motor_out[MOT_4] -= g.rc_4.pwm_out; // CW
motor_out[MOT_6] -= g.rc_4.pwm_out; // CW
motor_out[MOT_8] -= g.rc_4.pwm_out; // CW
}
// TODO add stability patch
motor_out[MOT_1] = min(motor_out[MOT_1], out_max);
motor_out[MOT_2] = min(motor_out[MOT_2], out_max);
motor_out[MOT_3] = min(motor_out[MOT_3], out_max);
motor_out[MOT_4] = min(motor_out[MOT_4], out_max);
motor_out[MOT_5] = min(motor_out[MOT_5], out_max);
motor_out[MOT_6] = min(motor_out[MOT_6], out_max);
motor_out[MOT_7] = min(motor_out[MOT_7], out_max);
motor_out[MOT_8] = min(motor_out[MOT_8], out_max);
motor_out[MOT_1] = min(motor_out[MOT_1], out_max);
motor_out[MOT_2] = min(motor_out[MOT_2], out_max);
motor_out[MOT_3] = min(motor_out[MOT_3], out_max);
motor_out[MOT_4] = min(motor_out[MOT_4], out_max);
motor_out[MOT_5] = min(motor_out[MOT_5], out_max);
motor_out[MOT_6] = min(motor_out[MOT_6], out_max);
motor_out[MOT_7] = min(motor_out[MOT_7], out_max);
motor_out[MOT_8] = min(motor_out[MOT_8], out_max);
// limit output so motors don't stop
motor_out[MOT_1] = max(motor_out[MOT_1], out_min);
motor_out[MOT_2] = max(motor_out[MOT_2], out_min);
motor_out[MOT_3] = max(motor_out[MOT_3], out_min);
motor_out[MOT_4] = max(motor_out[MOT_4], out_min);
motor_out[MOT_4] = max(motor_out[MOT_4], out_min);
motor_out[MOT_5] = max(motor_out[MOT_5], out_min);
motor_out[MOT_6] = max(motor_out[MOT_6], out_min);
motor_out[MOT_7] = max(motor_out[MOT_7], out_min);
motor_out[MOT_8] = max(motor_out[MOT_8], out_min);
motor_out[MOT_6] = max(motor_out[MOT_6], out_min);
motor_out[MOT_7] = max(motor_out[MOT_7], out_min);
motor_out[MOT_8] = max(motor_out[MOT_8], out_min);
#if CUT_MOTORS == ENABLED
// if we are not sending a throttle output, we cut the motors
if(g.rc_3.servo_out == 0){
motor_out[MOT_1] = g.rc_3.radio_min;
motor_out[MOT_2] = g.rc_3.radio_min;
motor_out[MOT_3] = g.rc_3.radio_min;
motor_out[MOT_1] = g.rc_3.radio_min;
motor_out[MOT_2] = g.rc_3.radio_min;
motor_out[MOT_3] = g.rc_3.radio_min;
motor_out[MOT_4] = g.rc_3.radio_min;
motor_out[MOT_5] = g.rc_3.radio_min;
motor_out[MOT_6] = g.rc_3.radio_min;
@ -114,8 +113,8 @@ static void output_motors_armed()
// this filter slows the acceleration of motors vs the deceleration
// Idea by Denny Rowland to help with his Yaw issue
for(int8_t m = 0; m <= 8; m++ ) {
int i = ch_of_mot(m);
for(int8_t m = 0; m <= 8; m++){
int i = ch_of_mot(m);
if(motor_filtered[i] < motor_out[i]){
motor_filtered[i] = (motor_out[i] + motor_filtered[i]) / 2;
}else{
@ -150,7 +149,7 @@ static void output_motors_disarmed()
}
// fill the motor_out[] array for HIL use
for (unsigned char i = 0; i < 11; i++) {
for (unsigned char i = 0; i < 11; i++){
motor_out[i] = g.rc_3.radio_min;
}
@ -167,37 +166,62 @@ static void output_motors_disarmed()
static void output_motor_test()
{
APM_RC.OutputCh(MOT_6, g.rc_3.radio_min);
APM_RC.OutputCh(MOT_7, g.rc_3.radio_min + 100);
delay(1000);
APM_RC.OutputCh(MOT_7, g.rc_3.radio_min);
APM_RC.OutputCh(MOT_8, g.rc_3.radio_min + 100);
delay(1000);
APM_RC.OutputCh(MOT_8, g.rc_3.radio_min);
APM_RC.OutputCh(MOT_1, g.rc_3.radio_min + 100);
delay(1000);
APM_RC.OutputCh(MOT_1, g.rc_3.radio_min);
APM_RC.OutputCh(MOT_2, g.rc_3.radio_min + 100);
delay(1000);
APM_RC.OutputCh(MOT_2, g.rc_3.radio_min);
APM_RC.OutputCh(MOT_3, g.rc_3.radio_min + 100);
delay(1000);
APM_RC.OutputCh(MOT_3, g.rc_3.radio_min);
APM_RC.OutputCh(MOT_4, g.rc_3.radio_min + 100);
delay(1000);
APM_RC.OutputCh(MOT_4, g.rc_3.radio_min);
APM_RC.OutputCh(MOT_5, g.rc_3.radio_min + 100);
delay(1000);
motor_out[MOT_1] = g.rc_3.radio_min;
motor_out[MOT_2] = g.rc_3.radio_min;
motor_out[MOT_3] = g.rc_3.radio_min;
motor_out[MOT_4] = g.rc_3.radio_min;
motor_out[MOT_5] = g.rc_3.radio_min;
motor_out[MOT_6] = g.rc_3.radio_min;
motor_out[MOT_7] = g.rc_3.radio_min;
motor_out[MOT_8] = g.rc_3.radio_min;
APM_RC.OutputCh(MOT_5, g.rc_3.radio_min);
APM_RC.OutputCh(MOT_6, g.rc_3.radio_min + 100);
delay(1000);
}
delay(5000);
APM_RC.OutputCh(MOT_1, g.rc_3.radio_min + 100);
delay(300);
APM_RC.OutputCh(MOT_1, g.rc_3.radio_min);
delay(3000);
APM_RC.OutputCh(MOT_6, g.rc_3.radio_min + 100);
delay(300);
APM_RC.OutputCh(MOT_6, g.rc_3.radio_min);
delay(3000);
APM_RC.OutputCh(MOT_4, g.rc_3.radio_min + 100);
delay(300);
APM_RC.OutputCh(MOT_4, g.rc_3.radio_min);
delay(3000);
APM_RC.OutputCh(MOT_7, g.rc_3.radio_min + 100);
delay(300);
APM_RC.OutputCh(MOT_7, g.rc_3.radio_min);
delay(3000);
APM_RC.OutputCh(MOT_3, g.rc_3.radio_min + 100);
delay(300);
APM_RC.OutputCh(MOT_3, g.rc_3.radio_min);
delay(3000);
APM_RC.OutputCh(MOT_8, g.rc_3.radio_min + 100);
delay(300);
APM_RC.OutputCh(MOT_8, g.rc_3.radio_min);
delay(3000);
APM_RC.OutputCh(MOT_2, g.rc_3.radio_min + 100);
delay(300);
APM_RC.OutputCh(MOT_2, g.rc_3.radio_min);
delay(3000);
APM_RC.OutputCh(MOT_5, g.rc_3.radio_min + 100);
delay(300);
APM_RC.OutputCh(MOT_1, motor_out[MOT_1]);
APM_RC.OutputCh(MOT_2, motor_out[MOT_2]);
APM_RC.OutputCh(MOT_3, motor_out[MOT_3]);
APM_RC.OutputCh(MOT_4, motor_out[MOT_4]);
APM_RC.OutputCh(MOT_5, motor_out[MOT_5]);
APM_RC.OutputCh(MOT_6, motor_out[MOT_6]);
APM_RC.OutputCh(MOT_7, motor_out[MOT_7]);
APM_RC.OutputCh(MOT_8, motor_out[MOT_8]);
}
#endif

134
ArduCopter/motors_quad.pde
View File

@ -5,16 +5,16 @@
static void init_motors_out()
{
#if INSTANT_PWM == 0
APM_RC.SetFastOutputChannels( _BV(MOT_1) | _BV(MOT_2) | _BV(MOT_3) | _BV(MOT_4) );
APM_RC.SetFastOutputChannels(_BV(MOT_1) | _BV(MOT_2) | _BV(MOT_3) | _BV(MOT_4));
#endif
}
static void motors_output_enable()
{
APM_RC.enable_out(MOT_1);
APM_RC.enable_out(MOT_2);
APM_RC.enable_out(MOT_3);
APM_RC.enable_out(MOT_4);
APM_RC.enable_out(MOT_1);
APM_RC.enable_out(MOT_2);
APM_RC.enable_out(MOT_3);
APM_RC.enable_out(MOT_4);
}
static void output_motors_armed()
@ -41,11 +41,11 @@ static void output_motors_armed()
pitch_out = g.rc_2.pwm_out * .707;
// left
motor_out[MOT_3] = g.rc_3.radio_out + roll_out + pitch_out; // FRONT
motor_out[MOT_2] = g.rc_3.radio_out + roll_out - pitch_out; // BACK
motor_out[MOT_3] = g.rc_3.radio_out + roll_out + pitch_out; // FRONT
motor_out[MOT_2] = g.rc_3.radio_out + roll_out - pitch_out; // BACK
// right
motor_out[MOT_1] = g.rc_3.radio_out - roll_out + pitch_out; // FRONT
motor_out[MOT_1] = g.rc_3.radio_out - roll_out + pitch_out; // FRONT
motor_out[MOT_4] = g.rc_3.radio_out - roll_out - pitch_out; // BACK
}else{
@ -54,54 +54,54 @@ static void output_motors_armed()
pitch_out = g.rc_2.pwm_out;
// right motor
motor_out[MOT_1] = g.rc_3.radio_out - roll_out;
motor_out[MOT_1] = g.rc_3.radio_out - roll_out;
// left motor
motor_out[MOT_2] = g.rc_3.radio_out + roll_out;
motor_out[MOT_2] = g.rc_3.radio_out + roll_out;
// front motor
motor_out[MOT_3] = g.rc_3.radio_out + pitch_out;
motor_out[MOT_3] = g.rc_3.radio_out + pitch_out;
// back motor
motor_out[MOT_4] = g.rc_3.radio_out - pitch_out;
}
// Yaw input
motor_out[MOT_1] += g.rc_4.pwm_out; // CCW
motor_out[MOT_2] += g.rc_4.pwm_out; // CCW
motor_out[MOT_3] -= g.rc_4.pwm_out; // CW
motor_out[MOT_4] -= g.rc_4.pwm_out; // CW
motor_out[MOT_1] += g.rc_4.pwm_out; // CCW
motor_out[MOT_2] += g.rc_4.pwm_out; // CCW
motor_out[MOT_3] -= g.rc_4.pwm_out; // CW
motor_out[MOT_4] -= g.rc_4.pwm_out; // CW
/* We need to clip motor output at out_max. When cipping a motors
* output we also need to compensate for the instability by
* lowering the opposite motor by the same proportion. This
* ensures that we retain control when one or more of the motors
* is at its maximum output
*/
for (int i=MOT_1; i<=MOT_4; i++) {
if (motor_out[i] > out_max) {
// note that i^1 is the opposite motor
motor_out[i^1] -= motor_out[i] - out_max;
motor_out[i] = out_max;
}
}
* output we also need to compensate for the instability by
* lowering the opposite motor by the same proportion. This
* ensures that we retain control when one or more of the motors
* is at its maximum output
*/
for (int i = MOT_1; i <= MOT_4; i++){
if(motor_out[i] > out_max){
// note that i^1 is the opposite motor
motor_out[i ^ 1] -= motor_out[i] - out_max;
motor_out[i] = out_max;
}
}
// limit output so motors don't stop
motor_out[MOT_1] = max(motor_out[MOT_1], out_min);
motor_out[MOT_2] = max(motor_out[MOT_2], out_min);
motor_out[MOT_3] = max(motor_out[MOT_3], out_min);
motor_out[MOT_1] = max(motor_out[MOT_1], out_min);
motor_out[MOT_2] = max(motor_out[MOT_2], out_min);
motor_out[MOT_3] = max(motor_out[MOT_3], out_min);
motor_out[MOT_4] = max(motor_out[MOT_4], out_min);
#if CUT_MOTORS == ENABLED
// if we are not sending a throttle output, we cut the motors
if(g.rc_3.servo_out == 0){
motor_out[MOT_1] = g.rc_3.radio_min;
motor_out[MOT_2] = g.rc_3.radio_min;
motor_out[MOT_3] = g.rc_3.radio_min;
motor_out[MOT_1] = g.rc_3.radio_min;
motor_out[MOT_2] = g.rc_3.radio_min;
motor_out[MOT_3] = g.rc_3.radio_min;
motor_out[MOT_4] = g.rc_3.radio_min;
}
#endif
// this filter slows the acceleration of motors vs the deceleration
// Idea by Denny Rowland to help with his Yaw issue
for(int8_t i=MOT_1; i <= MOT_4; i++ ) {
for(int8_t i = MOT_1; i <= MOT_4; i++){
if(motor_filtered[i] < motor_out[i]){
motor_filtered[i] = (motor_out[i] + motor_filtered[i]) / 2;
}else{
@ -133,7 +133,7 @@ static void output_motors_disarmed()
}
// fill the motor_out[] array for HIL use
for (unsigned char i = 0; i < 8; i++) {
for (unsigned char i = 0; i < 8; i++){
motor_out[i] = g.rc_3.radio_min;
}
@ -164,43 +164,49 @@ static void output_motor_test()
if(g.frame_orientation == X_FRAME){
// 31
// 24
if(g.rc_1.control_in > 3000){
motor_out[MOT_1] += 100;
motor_out[MOT_4] += 100;
}
if(g.rc_1.control_in < -3000){
motor_out[MOT_2] += 100;
motor_out[MOT_3] += 100;
}
APM_RC.OutputCh(MOT_3, g.rc_2.radio_min);
delay(4000);
APM_RC.OutputCh(MOT_1, g.rc_3.radio_min + 100);
delay(300);
if(g.rc_2.control_in > 3000){
motor_out[MOT_2] += 100;
motor_out[MOT_4] += 100;
}
APM_RC.OutputCh(MOT_1, g.rc_3.radio_min);
delay(2000);
APM_RC.OutputCh(MOT_4, g.rc_1.radio_min + 100);
delay(300);
if(g.rc_2.control_in < -3000){
motor_out[MOT_1] += 100;
motor_out[MOT_3] += 100;
}
APM_RC.OutputCh(MOT_4, g.rc_1.radio_min);
delay(2000);
APM_RC.OutputCh(MOT_2, g.rc_4.radio_min + 100);
delay(300);
APM_RC.OutputCh(MOT_2, g.rc_4.radio_min);
delay(2000);
APM_RC.OutputCh(MOT_3, g.rc_2.radio_min + 100);
delay(300);
}else{
// 3
// 2 1
// 4
if(g.rc_1.control_in > 3000)
motor_out[MOT_1] += 100;
if(g.rc_1.control_in < -3000)
motor_out[MOT_2] += 100;
APM_RC.OutputCh(MOT_3, g.rc_2.radio_min);
delay(4000);
APM_RC.OutputCh(MOT_1, g.rc_3.radio_min + 100);
delay(300);
if(g.rc_2.control_in > 3000)
motor_out[MOT_4] += 100;
APM_RC.OutputCh(MOT_1, g.rc_3.radio_min);
delay(2000);
APM_RC.OutputCh(MOT_2, g.rc_1.radio_min + 100);
delay(300);
APM_RC.OutputCh(MOT_2, g.rc_1.radio_min);
delay(2000);
APM_RC.OutputCh(MOT_4, g.rc_4.radio_min + 100);
delay(300);
APM_RC.OutputCh(MOT_4, g.rc_4.radio_min);
delay(2000);
APM_RC.OutputCh(MOT_3, g.rc_2.radio_min + 100);
delay(300);
if(g.rc_2.control_in < -3000)
motor_out[MOT_3] += 100;
}
APM_RC.OutputCh(MOT_1, motor_out[MOT_1]);

40
ArduCopter/motors_tri.pde
View File

@ -4,16 +4,16 @@
static void init_motors_out()
{
#if INSTANT_PWM == 0
APM_RC.SetFastOutputChannels( _BV(MOT_1) | _BV(MOT_2) | _BV(MOT_4) );
APM_RC.SetFastOutputChannels(_BV(MOT_1) | _BV(MOT_2) | _BV(MOT_4));
#endif
}
static void motors_output_enable()
{
APM_RC.enable_out(MOT_1);
APM_RC.enable_out(MOT_2);
APM_RC.enable_out(MOT_4);
APM_RC.enable_out(CH_TRI_YAW);
APM_RC.enable_out(MOT_1);
APM_RC.enable_out(MOT_2);
APM_RC.enable_out(MOT_4);
APM_RC.enable_out(CH_TRI_YAW);
}
@ -45,19 +45,19 @@ static void output_motors_armed()
//motor_out[MOT_4] += (float)(abs(g.rc_4.control_in)) * .013;
// Tridge's stability patch
if (motor_out[MOT_1] > out_max) {
if(motor_out[MOT_1] > out_max){
motor_out[MOT_2] -= (motor_out[MOT_1] - out_max) >> 1;
motor_out[MOT_4] -= (motor_out[MOT_1] - out_max) >> 1;
motor_out[MOT_1] = out_max;
}
if (motor_out[MOT_2] > out_max) {
if(motor_out[MOT_2] > out_max){
motor_out[MOT_1] -= (motor_out[MOT_2] - out_max) >> 1;
motor_out[MOT_4] -= (motor_out[MOT_2] - out_max) >> 1;
motor_out[MOT_2] = out_max;
}
if (motor_out[MOT_4] > out_max) {
if(motor_out[MOT_4] > out_max){
motor_out[MOT_1] -= (motor_out[MOT_4] - out_max) >> 1;
motor_out[MOT_2] -= (motor_out[MOT_4] - out_max) >> 1;
motor_out[MOT_4] = out_max;
@ -97,7 +97,7 @@ static void output_motors_disarmed()
}
// fill the motor_out[] array for HIL use
for (unsigned char i = 0; i < 8; i++) {
for (unsigned char i = 0; i < 8; i++){
motor_out[i] = g.rc_3.radio_min;
}
@ -113,18 +113,20 @@ static void output_motor_test()
motor_out[MOT_2] = g.rc_3.radio_min;
motor_out[MOT_4] = g.rc_3.radio_min;
APM_RC.OutputCh(MOT_2, g.rc_2.radio_min);
delay(4000);
APM_RC.OutputCh(MOT_1, g.rc_3.radio_min + 100);
delay(300);
if(g.rc_1.control_in > 3000){ // right
motor_out[MOT_1] += 100;
}
APM_RC.OutputCh(MOT_1, g.rc_3.radio_min);
delay(2000);
APM_RC.OutputCh(MOT_4, g.rc_1.radio_min + 100);
delay(300);
if(g.rc_1.control_in < -3000){ // left
motor_out[MOT_2] += 100;
}
if(g.rc_2.control_in > 3000){ // back
motor_out[MOT_4] += 100;
}
APM_RC.OutputCh(MOT_4, g.rc_1.radio_min);
delay(2000);
APM_RC.OutputCh(MOT_2, g.rc_4.radio_min + 100);
delay(300);
APM_RC.OutputCh(MOT_1, motor_out[MOT_1]);
APM_RC.OutputCh(MOT_2, motor_out[MOT_2]);

114
ArduCopter/motors_y6.pde
View File

@ -7,19 +7,19 @@
static void init_motors_out()
{
#if INSTANT_PWM == 0
APM_RC.SetFastOutputChannels( _BV(MOT_1) | _BV(MOT_2) | _BV(MOT_3) | _BV(MOT_4)
| _BV(MOT_5) | _BV(MOT_6) );
APM_RC.SetFastOutputChannels(_BV(MOT_1) | _BV(MOT_2) | _BV(MOT_3) | _BV(MOT_4)
| _BV(MOT_5) | _BV(MOT_6));
#endif
}
static void motors_output_enable()
{
APM_RC.enable_out(MOT_1);
APM_RC.enable_out(MOT_2);
APM_RC.enable_out(MOT_3);
APM_RC.enable_out(MOT_4);
APM_RC.enable_out(MOT_5);
APM_RC.enable_out(MOT_6);
APM_RC.enable_out(MOT_1);
APM_RC.enable_out(MOT_2);
APM_RC.enable_out(MOT_3);
APM_RC.enable_out(MOT_4);
APM_RC.enable_out(MOT_5);
APM_RC.enable_out(MOT_6);
}
static void output_motors_armed()
@ -40,41 +40,41 @@ static void output_motors_armed()
// Multi-Wii Mix
//left
motor_out[MOT_2] = (g.rc_3.radio_out * g.top_bottom_ratio) + g.rc_1.pwm_out + (g.rc_2.pwm_out * 2 / 3); // LEFT TOP - CW
motor_out[MOT_3] = g.rc_3.radio_out + g.rc_1.pwm_out + (g.rc_2.pwm_out * 2 / 3); // BOTTOM_LEFT - CCW
motor_out[MOT_2] = (g.rc_3.radio_out * g.top_bottom_ratio) + g.rc_1.pwm_out + (g.rc_2.pwm_out * 2 / 3); // LEFT TOP - CW
motor_out[MOT_3] = g.rc_3.radio_out + g.rc_1.pwm_out + (g.rc_2.pwm_out * 2 / 3); // BOTTOM_LEFT - CCW
//right
motor_out[MOT_5] = (g.rc_3.radio_out * g.top_bottom_ratio) - g.rc_1.pwm_out + (g.rc_2.pwm_out * 2 / 3); // RIGHT TOP - CW
motor_out[MOT_1] = g.rc_3.radio_out - g.rc_1.pwm_out + (g.rc_2.pwm_out * 2 / 3); // BOTTOM_RIGHT - CCW
motor_out[MOT_5] = (g.rc_3.radio_out * g.top_bottom_ratio) - g.rc_1.pwm_out + (g.rc_2.pwm_out * 2 / 3); // RIGHT TOP - CW
motor_out[MOT_1] = g.rc_3.radio_out - g.rc_1.pwm_out + (g.rc_2.pwm_out * 2 / 3); // BOTTOM_RIGHT - CCW
//back
motor_out[MOT_6] = (g.rc_3.radio_out * g.top_bottom_ratio) - (g.rc_2.pwm_out * 4 / 3); // REAR TOP - CCW
motor_out[MOT_4] = g.rc_3.radio_out - (g.rc_2.pwm_out * 4 / 3); // BOTTOM_REAR - CW
motor_out[MOT_6] = (g.rc_3.radio_out * g.top_bottom_ratio) - (g.rc_2.pwm_out * 4 / 3); // REAR TOP - CCW
motor_out[MOT_4] = g.rc_3.radio_out - (g.rc_2.pwm_out * 4 / 3); // BOTTOM_REAR - CW
//left
motor_out[MOT_2] -= YAW_DIRECTION * g.rc_4.pwm_out; // LEFT TOP - CW
motor_out[MOT_3] += YAW_DIRECTION * g.rc_4.pwm_out; // LEFT BOTTOM - CCW
motor_out[MOT_2] -= YAW_DIRECTION * g.rc_4.pwm_out; // LEFT TOP - CW
motor_out[MOT_3] += YAW_DIRECTION * g.rc_4.pwm_out; // LEFT BOTTOM - CCW
//right
motor_out[MOT_5] -= YAW_DIRECTION * g.rc_4.pwm_out; // RIGHT TOP - CW
motor_out[MOT_1] += YAW_DIRECTION * g.rc_4.pwm_out; // RIGHT BOTTOM - CCW
motor_out[MOT_5] -= YAW_DIRECTION * g.rc_4.pwm_out; // RIGHT TOP - CW
motor_out[MOT_1] += YAW_DIRECTION * g.rc_4.pwm_out; // RIGHT BOTTOM - CCW
//back
motor_out[MOT_6] += YAW_DIRECTION * g.rc_4.pwm_out; // REAR TOP - CCW
motor_out[MOT_4] -= YAW_DIRECTION * g.rc_4.pwm_out; // REAR BOTTOM - CW
motor_out[MOT_6] += YAW_DIRECTION * g.rc_4.pwm_out; // REAR TOP - CCW
motor_out[MOT_4] -= YAW_DIRECTION * g.rc_4.pwm_out; // REAR BOTTOM - CW
/*
int roll_out = (float)g.rc_1.pwm_out * .866;
int pitch_out = g.rc_2.pwm_out / 2;
int pitch_out = g.rc_2.pwm_out / 2;
//left
motor_out[MOT_2] = ((g.rc_3.radio_out * g.top_bottom_ratio) + roll_out + pitch_out); // CCW TOP
motor_out[MOT_3] = g.rc_3.radio_out + roll_out + pitch_out; // CW
motor_out[MOT_2] = ((g.rc_3.radio_out * g.top_bottom_ratio) + roll_out + pitch_out); // CCW TOP
motor_out[MOT_3] = g.rc_3.radio_out + roll_out + pitch_out; // CW
//right
motor_out[MOT_5] = ((g.rc_3.radio_out * g.top_bottom_ratio) - roll_out + pitch_out); // CCW TOP
motor_out[MOT_1] = g.rc_3.radio_out - roll_out + pitch_out; // CW
motor_out[MOT_1] = g.rc_3.radio_out - roll_out + pitch_out; // CW
//back
motor_out[MOT_6] = ((g.rc_3.radio_out * g.top_bottom_ratio) - g.rc_2.pwm_out); // CCW TOP
motor_out[MOT_4] = g.rc_3.radio_out - g.rc_2.pwm_out; // CW
motor_out[MOT_6] = ((g.rc_3.radio_out * g.top_bottom_ratio) - g.rc_2.pwm_out); // CCW TOP
motor_out[MOT_4] = g.rc_3.radio_out - g.rc_2.pwm_out; // CW
// Yaw
//top
@ -85,7 +85,7 @@ static void output_motors_armed()
//bottom
motor_out[MOT_3] -= g.rc_4.pwm_out; // CW
motor_out[MOT_1] -= g.rc_4.pwm_out; // CW
motor_out[MOT_4] -= g.rc_4.pwm_out; // CW
motor_out[MOT_4] -= g.rc_4.pwm_out; // CW
*/
// TODO: add stability patch
@ -93,16 +93,16 @@ static void output_motors_armed()
motor_out[MOT_2] = min(motor_out[MOT_2], out_max);
motor_out[MOT_3] = min(motor_out[MOT_3], out_max);
motor_out[MOT_4] = min(motor_out[MOT_4], out_max);
motor_out[MOT_5] = min(motor_out[MOT_5], out_max);
motor_out[MOT_6] = min(motor_out[MOT_6], out_max);
motor_out[MOT_5] = min(motor_out[MOT_5], out_max);
motor_out[MOT_6] = min(motor_out[MOT_6], out_max);
// limit output so motors don't stop
motor_out[MOT_1] = max(motor_out[MOT_1], out_min);
motor_out[MOT_2] = max(motor_out[MOT_2], out_min);
motor_out[MOT_3] = max(motor_out[MOT_3], out_min);
motor_out[MOT_4] = max(motor_out[MOT_4], out_min);
motor_out[MOT_5] = max(motor_out[MOT_5], out_min);
motor_out[MOT_6] = max(motor_out[MOT_6], out_min);
motor_out[MOT_1] = max(motor_out[MOT_1], out_min);
motor_out[MOT_2] = max(motor_out[MOT_2], out_min);
motor_out[MOT_3] = max(motor_out[MOT_3], out_min);
motor_out[MOT_4] = max(motor_out[MOT_4], out_min);
motor_out[MOT_5] = max(motor_out[MOT_5], out_min);
motor_out[MOT_6] = max(motor_out[MOT_6], out_min);
#if CUT_MOTORS == ENABLED
// if we are not sending a throttle output, we cut the motors
@ -118,8 +118,8 @@ static void output_motors_armed()
// this filter slows the acceleration of motors vs the deceleration
// Idea by Denny Rowland to help with his Yaw issue
for(int8_t m = 0; m <= 6; m++ ) {
int i = ch_of_mot(m);
for(int8_t m = 0; m <= 6; m++){
int i = ch_of_mot(m);
if(motor_filtered[i] < motor_out[i]){
motor_filtered[i] = (motor_out[i] + motor_filtered[i]) / 2;
}else{
@ -152,7 +152,7 @@ static void output_motors_disarmed()
}
// fill the motor_out[] array for HIL use
for (unsigned char i = 0; i < 8; i++) {
for (unsigned char i = 0; i < 8; i++){
motor_out[i] = g.rc_3.radio_min;
}
@ -174,21 +174,35 @@ static void output_motor_test()
motor_out[MOT_5] = g.rc_3.radio_min;
motor_out[MOT_6] = g.rc_3.radio_min;
APM_RC.OutputCh(MOT_1, g.rc_3.radio_min);
delay(5000);
APM_RC.OutputCh(MOT_2, g.rc_3.radio_min + 100);
delay(300);
if(g.rc_1.control_in > 3000){ // right
motor_out[MOT_1] += 100;
motor_out[MOT_5] += 100;
}
APM_RC.OutputCh(MOT_2, g.rc_3.radio_min);
delay(3000);
APM_RC.OutputCh(MOT_3, g.rc_3.radio_min + 100);
delay(300);
if(g.rc_1.control_in < -3000){ // left
motor_out[MOT_2] += 100;
motor_out[MOT_3] += 100;
}
APM_RC.OutputCh(MOT_3, g.rc_3.radio_min);
delay(3000);
APM_RC.OutputCh(MOT_6, g.rc_3.radio_min + 100);
delay(300);
if(g.rc_2.control_in > 3000){ // back
motor_out[MOT_6] += 100;
motor_out[MOT_4] += 100;
}
APM_RC.OutputCh(MOT_6, g.rc_3.radio_min);
delay(3000);
APM_RC.OutputCh(MOT_4, g.rc_3.radio_min + 100);
delay(300);
APM_RC.OutputCh(MOT_4, g.rc_3.radio_min);
delay(3000);
APM_RC.OutputCh(MOT_5, g.rc_3.radio_min + 100);
delay(300);
APM_RC.OutputCh(MOT_5, g.rc_3.radio_min);
delay(3000);
APM_RC.OutputCh(MOT_1, g.rc_3.radio_min + 100);
delay(300);
APM_RC.OutputCh(MOT_1, motor_out[MOT_1]);
APM_RC.OutputCh(MOT_2, motor_out[MOT_2]);

26
ArduCopter/navigation.pde
View File

@ -117,8 +117,12 @@ static void calc_loiter(int x_error, int y_error)
int16_t y_iterm = g.pi_loiter_lat.get_i(y_error, dTnav);
y_rate_error = y_target_speed - y_actual_speed;
calc_nav_lon(x_rate_error);
calc_nav_lat(y_rate_error);
//nav_lon = g.pid_loiter_rate_lon.get_pid(x_rate_error, dTnav);
//nav_lat = g.pid_loiter_rate_lat.get_pid(y_rate_error, dTnav);
nav_lon = g.pid_nav_lon.get_pid(x_rate_error, dTnav);
nav_lat = g.pid_nav_lat.get_pid(y_rate_error, dTnav);
nav_lon = constrain(nav_lon, -3000, 3000);
nav_lat = constrain(nav_lat, -3000, 3000);
nav_lat = nav_lat + y_iterm;
nav_lon = nav_lon + x_iterm;
@ -176,8 +180,11 @@ static void calc_nav_rate(int max_speed)
y_rate_error = constrain(y_rate_error, -1000, 1000); // added a rate error limit to keep pitching down to a minimum
int16_t y_iterm = g.pi_loiter_lat.get_i(y_rate_error, dTnav);
calc_nav_lon(x_rate_error);
calc_nav_lat(y_rate_error);
nav_lon = g.pid_nav_lon.get_pid(x_rate_error, dTnav);
nav_lat = g.pid_nav_lat.get_pid(y_rate_error, dTnav);
nav_lon = constrain(nav_lon, -3000, 3000);
nav_lat = constrain(nav_lat, -3000, 3000);
nav_lon = nav_lon + x_iterm;
nav_lat = nav_lat + y_iterm;
@ -209,19 +216,18 @@ static void calc_nav_rate(int max_speed)
}
static void calc_nav_lon(int rate)
/*static void calc_nav_lon(int rate)
{
nav_lon = g.pid_nav_lon.get_pid(rate, dTnav);
//nav_lon = get_corrected_angle(rate, nav_lon);
nav_lon = constrain(nav_lon, -3000, 3000);
}
static void calc_nav_lat(int rate)
{
nav_lat = g.pid_nav_lat.get_pid(rate, dTnav);
//nav_lat = get_corrected_angle(rate, nav_lat);
nav_lat = constrain(nav_lat, -3000, 3000);
}
*/
//static int16_t get_corrected_angle(int16_t desired_rate, int16_t rate_out)
/*{
@ -252,11 +258,11 @@ static void calc_loiter_pitch_roll()
{
//Serial.printf("ys %ld, cx %1.4f, _cx %1.4f | sy %1.4f, _sy %1.4f\n", dcm.yaw_sensor, cos_yaw_x, _cos_yaw_x, sin_yaw_y, _sin_yaw_y);
// rotate the vector
nav_roll = (float)nav_lon * sin_yaw_y - (float)nav_lat * cos_yaw_x;
nav_pitch = (float)nav_lon * cos_yaw_x + (float)nav_lat * sin_yaw_y;
auto_roll = (float)nav_lon * sin_yaw_y - (float)nav_lat * cos_yaw_x;
auto_pitch = (float)nav_lon * cos_yaw_x + (float)nav_lat * sin_yaw_y;
// flip pitch because forward is negative
nav_pitch = -nav_pitch;
auto_pitch = -auto_pitch;
}
static int16_t calc_desired_speed(int16_t max_speed)

28
ArduCopter/setup.pde
View File

@ -14,6 +14,7 @@ static int8_t setup_batt_monitor (uint8_t argc, const Menu::arg *argv);
static int8_t setup_sonar (uint8_t argc, const Menu::arg *argv);
static int8_t setup_compass (uint8_t argc, const Menu::arg *argv);
static int8_t setup_tune (uint8_t argc, const Menu::arg *argv);
static int8_t setup_range (uint8_t argc, const Menu::arg *argv);
//static int8_t setup_mag_offset (uint8_t argc, const Menu::arg *argv);
static int8_t setup_declination (uint8_t argc, const Menu::arg *argv);
static int8_t setup_optflow (uint8_t argc, const Menu::arg *argv);
@ -39,6 +40,7 @@ const struct Menu::command setup_menu_commands[] PROGMEM = {
{"sonar", setup_sonar},
{"compass", setup_compass},
{"tune", setup_tune},
{"range", setup_range},
// {"offsets", setup_mag_offset},
{"declination", setup_declination},
{"optflow", setup_optflow},
@ -102,7 +104,8 @@ setup_show(uint8_t argc, const Menu::arg *argv)
report_gyro();
#endif
AP_Var_menu_show(argc, argv);
AP_Param::show_all();
return(0);
}
@ -122,7 +125,7 @@ setup_factory(uint8_t argc, const Menu::arg *argv)
if (('y' != c) && ('Y' != c))
return(-1);
AP_Var::erase_all();
AP_Param::erase_all();
Serial.printf_P(PSTR("\nReboot APM"));
delay(1000);
@ -224,6 +227,10 @@ setup_radio(uint8_t argc, const Menu::arg *argv)
static int8_t
setup_motors(uint8_t argc, const Menu::arg *argv)
{
Serial.printf_P(PSTR(
"Now connect the main lipo and follow the instruction on the wiki for your frame setup.\n"
"For security remember to disconnect the main lipo after the test, then hit any key to exit.\n"
"Any key to exit.\n"));
while(1){
delay(20);
read_radio();
@ -350,6 +357,19 @@ static int8_t
setup_tune(uint8_t argc, const Menu::arg *argv)
{
g.radio_tuning.set_and_save(argv[1].i);
//g.radio_tuning_high.set_and_save(1000);
//g.radio_tuning_low.set_and_save(0);
report_tuning();
return 0;
}
static int8_t
setup_range(uint8_t argc, const Menu::arg *argv)
{
Serial.printf_P(PSTR("\nCH 6 Ranges are divided by 1000: [low, high]\n"));
g.radio_tuning_low.set_and_save(argv[1].i);
g.radio_tuning_high.set_and_save(argv[2].i);
report_tuning();
return 0;
}
@ -1154,7 +1174,9 @@ static void report_tuning()
if (g.radio_tuning == 0){
print_enabled(g.radio_tuning.get());
}else{
Serial.printf_P(PSTR(" %d\n"),(int)g.radio_tuning.get());
float low = (float)g.radio_tuning_low.get() / 1000;
float high = (float)g.radio_tuning_high.get() / 1000;
Serial.printf_P(PSTR(" %d, Low:%1.4f, High:%1.4f\n"),(int)g.radio_tuning.get(), low, high);
}
print_blanks(2);
}

30
ArduCopter/system.pde
View File

@ -159,34 +159,8 @@ static void init_ardupilot()
piezo_beep();
#endif
if (!g.format_version.load() ||
g.format_version != Parameters::k_format_version) {
//Serial.printf_P(PSTR("\n\nForcing complete parameter reset..."));
/*Serial.printf_P(PSTR("\n\nEEPROM format version %d not compatible with this firmware (requires %d)"
"\n\nForcing complete parameter reset..."),
g.format_version.get(),
Parameters::k_format_version);
*/
// erase all parameters
Serial.printf_P(PSTR("Firmware change: erasing EEPROM...\n"));
delay(100); // wait for serial send
AP_Var::erase_all();
// save the new format version
g.format_version.set_and_save(Parameters::k_format_version);
// save default radio values
default_dead_zones();
}else{
// save default radio values
//default_dead_zones();
// Load all auto-loaded EEPROM variables
AP_Var::load_all();
}
// load parameters from EEPROM
load_parameters();
// init the GCS
gcs0.init(&Serial);

20
ArduPlane/ArduPlane.pde
View File

@ -150,7 +150,7 @@ static AP_Baro_BMP085 barometer(false);
static AP_Baro_MS5611 barometer;
#endif
static AP_Compass_HMC5843 compass(Parameters::k_param_compass);
static AP_Compass_HMC5843 compass;
#endif
// real GPS selection
@ -184,7 +184,7 @@ AP_GPS_None g_gps_driver(NULL);
# else
AP_InertialSensor_Oilpan ins( &adc );
#endif // CONFIG_IMU_TYPE
AP_IMU_INS imu( &ins, Parameters::k_param_IMU_calibration );
AP_IMU_INS imu( &ins );
AP_DCM dcm(&imu, g_gps);
#elif HIL_MODE == HIL_MODE_SENSORS
@ -217,8 +217,8 @@ AP_TimerProcess timer_scheduler;
// GCS selection
////////////////////////////////////////////////////////////////////////////////
//
GCS_MAVLINK gcs0(Parameters::k_param_streamrates_port0);
GCS_MAVLINK gcs3(Parameters::k_param_streamrates_port3);
GCS_MAVLINK gcs0;
GCS_MAVLINK gcs3;
////////////////////////////////////////////////////////////////////////////////
// PITOT selection
@ -979,7 +979,7 @@ static void update_current_flight_mode(void)
switch(nav_command_ID){
case MAV_CMD_NAV_TAKEOFF:
if (hold_course > -1) {
if (hold_course != -1) {
calc_nav_roll();
} else {
nav_roll = 0;
@ -1012,12 +1012,16 @@ static void update_current_flight_mode(void)
nav_pitch = landing_pitch; // pitch held constant
}
if (land_complete){
if (land_complete) {
// we are in the final stage of a landing - force
// zero throttle
g.channel_throttle.servo_out = 0;
}
break;
default:
// we are doing normal AUTO flight, the special cases
// are for takeoff and landing
hold_course = -1;
calc_nav_roll();
calc_nav_pitch();
@ -1025,11 +1029,13 @@ static void update_current_flight_mode(void)
break;
}
}else{
// hold_course is only used in takeoff and landing
hold_course = -1;
switch(control_mode){
case RTL:
case LOITER:
case GUIDED:
hold_course = -1;
crash_checker();
calc_nav_roll();
calc_nav_pitch();

11
ArduPlane/GCS.h
View File

@ -108,7 +108,7 @@ protected:
class GCS_MAVLINK : public GCS_Class
{
public:
GCS_MAVLINK(AP_Var::Key key);
GCS_MAVLINK();
void update(void);
void init(FastSerial *port);
void send_message(enum ap_message id);
@ -118,13 +118,16 @@ public:
void queued_param_send();
void queued_waypoint_send();
static const struct AP_Param::GroupInfo var_info[];
private:
void handleMessage(mavlink_message_t * msg);
/// Perform queued sending operations
///
AP_Var *_queued_parameter; ///< next parameter to be sent in queue
AP_Param *_queued_parameter; ///< next parameter to be sent in queue
enum ap_var_type _queued_parameter_type; ///< type of the next parameter
uint16_t _queued_parameter_token; ///AP_Param token for next() call
uint16_t _queued_parameter_index; ///< next queued parameter's index
uint16_t _queued_parameter_count; ///< saved count of parameters for queued send
@ -139,7 +142,6 @@ private:
uint16_t _count_parameters(); ///< count reportable parameters
uint16_t _parameter_count; ///< cache of reportable parameters
AP_Var *_find_parameter(uint16_t index); ///< find a reportable parameter by index
mavlink_channel_t chan;
uint16_t packet_drops;
@ -163,7 +165,6 @@ private:
uint16_t waypoint_receive_timeout; // milliseconds
// data stream rates
AP_Var_group _group;
AP_Int16 streamRateRawSensors;
AP_Int16 streamRateExtendedStatus;
AP_Int16 streamRateRCChannels;

160
ArduPlane/GCS_Mavlink.pde
View File

@ -113,7 +113,7 @@ static NOINLINE void send_attitude(mavlink_channel_t chan)
chan,
micros(),
dcm.roll,
dcm.pitch,
dcm.pitch - radians(g.pitch_trim*0.01),
dcm.yaw,
omega.x,
omega.y,
@ -213,6 +213,13 @@ static NOINLINE void send_extended_status1(mavlink_channel_t chan, uint16_t pack
battery_current = current_amps1 * 100;
}
if (g.battery_monitoring == 3) {
/*setting a out-of-range value.
It informs to external devices that
it cannot be calculated properly just by voltage*/
battery_remaining = 150;
}
mavlink_msg_sys_status_send(
chan,
control_sensors_present,
@ -272,6 +279,13 @@ static NOINLINE void send_extended_status1(mavlink_channel_t chan, uint16_t pack
uint8_t status = MAV_STATE_ACTIVE;
uint16_t battery_remaining = 1000.0 * (float)(g.pack_capacity - current_total1)/(float)g.pack_capacity; //Mavlink scaling 100% = 1000
if (g.battery_monitoring == 3) {
/*setting a out-of-range value.
It informs to external devices that
it cannot be calculated properly just by voltage*/
battery_remaining = 1500;
}
mavlink_msg_sys_status_send(
chan,
mode,
@ -308,12 +322,13 @@ static void NOINLINE send_location(mavlink_channel_t chan)
static void NOINLINE send_nav_controller_output(mavlink_channel_t chan)
{
int16_t bearing = (hold_course==-1?nav_bearing:hold_course) / 100;
mavlink_msg_nav_controller_output_send(
chan,
nav_roll / 1.0e2,
nav_pitch / 1.0e2,
nav_bearing / 1.0e2,
target_bearing / 1.0e2,
bearing,
target_bearing / 100,
wp_distance,
altitude_error / 1.0e2,
airspeed_error,
@ -715,28 +730,24 @@ void mavlink_send_text(mavlink_channel_t chan, gcs_severity severity, const char
}
}
const AP_Param::GroupInfo GCS_MAVLINK::var_info[] PROGMEM = {
AP_GROUPINFO("RAW_SENS", 0, GCS_MAVLINK, streamRateRawSensors),
AP_GROUPINFO("EXT_STAT", 1, GCS_MAVLINK, streamRateExtendedStatus),
AP_GROUPINFO("RC_CHAN", 2, GCS_MAVLINK, streamRateRCChannels),
AP_GROUPINFO("RAW_CTRL", 3, GCS_MAVLINK, streamRateRawController),
AP_GROUPINFO("POSITION", 4, GCS_MAVLINK, streamRatePosition),
AP_GROUPINFO("EXTRA1", 5, GCS_MAVLINK, streamRateExtra1),
AP_GROUPINFO("EXTRA2", 6, GCS_MAVLINK, streamRateExtra2),
AP_GROUPINFO("EXTRA3", 7, GCS_MAVLINK, streamRateExtra3),
AP_GROUPEND
};
GCS_MAVLINK::GCS_MAVLINK(AP_Var::Key key) :
packet_drops(0),
// parameters
// note, all values not explicitly initialised here are zeroed
waypoint_send_timeout(1000), // 1 second
waypoint_receive_timeout(1000), // 1 second
// stream rates
_group (key, key == Parameters::k_param_streamrates_port0 ? PSTR("SR0_"): PSTR("SR3_")),
// AP_VAR //ref //index, default, name
streamRateRawSensors (&_group, 0, 0, PSTR("RAW_SENS")),
streamRateExtendedStatus (&_group, 1, 0, PSTR("EXT_STAT")),
streamRateRCChannels (&_group, 2, 0, PSTR("RC_CHAN")),
streamRateRawController (&_group, 3, 0, PSTR("RAW_CTRL")),
streamRatePosition (&_group, 4, 0, PSTR("POSITION")),
streamRateExtra1 (&_group, 5, 0, PSTR("EXTRA1")),
streamRateExtra2 (&_group, 6, 0, PSTR("EXTRA2")),
streamRateExtra3 (&_group, 7, 0, PSTR("EXTRA3"))
GCS_MAVLINK::GCS_MAVLINK() :
packet_drops(0),
waypoint_send_timeout(1000), // 1 second
waypoint_receive_timeout(1000) // 1 second
{
}
void
@ -1099,12 +1110,14 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
break;
case MAV_ACTION_STORAGE_READ:
AP_Var::load_all();
// we load all variables at startup, and
// save on each mavlink set
result=1;
break;
case MAV_ACTION_STORAGE_WRITE:
AP_Var::save_all();
// this doesn't make any sense, as we save
// all settings as they come in
result=1;
break;
@ -1394,7 +1407,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
// Start sending parameters - next call to ::update will kick the first one out
_queued_parameter = AP_Var::first();
_queued_parameter = AP_Param::first(&_queued_parameter_token, &_queued_parameter_type);
_queued_parameter_index = 0;
_queued_parameter_count = _count_parameters();
break;
@ -1689,8 +1702,8 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
case MAVLINK_MSG_ID_PARAM_SET:
{
AP_Var *vp;
AP_Meta_class::Type_id var_type;
AP_Param *vp;
enum ap_var_type var_type;
// decode
mavlink_param_set_t packet;
@ -1706,7 +1719,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
key[ONBOARD_PARAM_NAME_LENGTH] = 0;
// find the requested parameter
vp = AP_Var::find(key);
vp = AP_Param::find(key, &var_type);
if ((NULL != vp) && // exists
!isnan(packet.param_value) && // not nan
!isinf(packet.param_value)) { // not inf
@ -1716,21 +1729,16 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
// next lower integer value.
float rounding_addition = 0.01;
// fetch the variable type ID
var_type = vp->meta_type_id();
// handle variables with standard type IDs
if (var_type == AP_Var::k_typeid_float) {
if (var_type == AP_PARAM_FLOAT) {
((AP_Float *)vp)->set_and_save(packet.param_value);
} else if (var_type == AP_Var::k_typeid_float16) {
((AP_Float16 *)vp)->set_and_save(packet.param_value);
} else if (var_type == AP_Var::k_typeid_int32) {
} else if (var_type == AP_PARAM_INT32) {
if (packet.param_value < 0) rounding_addition = -rounding_addition;
((AP_Int32 *)vp)->set_and_save(packet.param_value+rounding_addition);
} else if (var_type == AP_Var::k_typeid_int16) {
} else if (var_type == AP_PARAM_INT16) {
if (packet.param_value < 0) rounding_addition = -rounding_addition;
((AP_Int16 *)vp)->set_and_save(packet.param_value+rounding_addition);
} else if (var_type == AP_Var::k_typeid_int8) {
} else if (var_type == AP_PARAM_INT8) {
if (packet.param_value < 0) rounding_addition = -rounding_addition;
((AP_Int8 *)vp)->set_and_save(packet.param_value+rounding_addition);
} else {
@ -1745,8 +1753,8 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
mavlink_msg_param_value_send(
chan,
key,
vp->cast_to_float(),
mav_var_type(vp->meta_type_id()),
vp->cast_to_float(var_type),
mav_var_type(var_type),
_count_parameters(),
-1); // XXX we don't actually know what its index is...
}
@ -1961,44 +1969,17 @@ GCS_MAVLINK::_count_parameters()
{
// if we haven't cached the parameter count yet...
if (0 == _parameter_count) {
AP_Var *vp;
AP_Param *vp;
uint16_t token;
vp = AP_Var::first();
vp = AP_Param::first(&token, NULL);
do {
// if a parameter responds to cast_to_float then we are going to be able to report it
if (!isnan(vp->cast_to_float())) {
_parameter_count++;
}
} while (NULL != (vp = vp->next()));
_parameter_count++;
} while (NULL != (vp = AP_Param::next_scalar(&token, NULL)));
}
return _parameter_count;
}
AP_Var *
GCS_MAVLINK::_find_parameter(uint16_t index)
{
AP_Var *vp;
vp = AP_Var::first();
while (NULL != vp) {
// if the parameter is reportable
if (!(isnan(vp->cast_to_float()))) {
// if we have counted down to the index we want
if (0 == index) {
// return the parameter
return vp;
}
// count off this parameter, as it is reportable but not
// the one we want
index--;
}
// and move to the next parameter
vp = vp->next();
}
return NULL;
}
/**
* @brief Send the next pending parameter, called from deferred message
* handling code
@ -2009,29 +1990,28 @@ GCS_MAVLINK::queued_param_send()
// Check to see if we are sending parameters
if (NULL == _queued_parameter) return;
AP_Var *vp;
AP_Param *vp;
float value;
// copy the current parameter and prepare to move to the next
vp = _queued_parameter;
_queued_parameter = _queued_parameter->next();
// if the parameter can be cast to float, report it here and break out of the loop
value = vp->cast_to_float();
if (!isnan(value)) {
char param_name[ONBOARD_PARAM_NAME_LENGTH]; /// XXX HACK
vp->copy_name(param_name, sizeof(param_name));
value = vp->cast_to_float(_queued_parameter_type);
mavlink_msg_param_value_send(
chan,
param_name,
value,
mav_var_type(vp->meta_type_id()),
_queued_parameter_count,
_queued_parameter_index);
char param_name[ONBOARD_PARAM_NAME_LENGTH];
vp->copy_name(param_name, sizeof(param_name));
_queued_parameter_index++;
}
mavlink_msg_param_value_send(
chan,
param_name,
value,
mav_var_type(_queued_parameter_type),
_queued_parameter_count,
_queued_parameter_index);
_queued_parameter = AP_Param::next_scalar(&_queued_parameter_token, &_queued_parameter_type);
_queued_parameter_index++;
}
/**
@ -2125,14 +2105,6 @@ static void gcs_update(void)
}
}
static void gcs_send_text(gcs_severity severity, const char *str)
{
gcs0.send_text(severity, str);
if (gcs3.initialised) {
gcs3.send_text(severity, str);
}
}
static void gcs_send_text_P(gcs_severity severity, const prog_char_t *str)
{
gcs0.send_text(severity, str);

232
ArduPlane/Parameters.h
View File

@ -26,36 +26,14 @@ public:
//
static const uint16_t k_software_type = 0; // 0 for APM trunk
//
// Parameter identities.
//
// The enumeration defined here is used to ensure that every parameter
// or parameter group has a unique ID number. This number is used by
// AP_Var to store and locate parameters in EEPROM.
//
// Note that entries without a number are assigned the next number after
// the entry preceding them. When adding new entries, ensure that they
// don't overlap.
//
// Try to group related variables together, and assign them a set
// range in the enumeration. Place these groups in numerical order
// at the end of the enumeration.
//
// WARNING: Care should be taken when editing this enumeration as the
// AP_Var load/save code depends on the values here to identify
// variables saved in EEPROM.
//
//
enum {
// Layout version number, always key zero.
//
k_param_format_version = 0,
k_param_software_type,
// Misc
//
k_param_auto_trim,
k_param_switch_enable,
k_param_log_bitmask,
@ -75,8 +53,8 @@ public:
// 110: Telemetry control
//
k_param_streamrates_port0 = 110,
k_param_streamrates_port3,
k_param_gcs0 = 110, // stream rates for port0
k_param_gcs3, // stream rates for port3
k_param_sysid_this_mav,
k_param_sysid_my_gcs,
k_param_serial3_baud,
@ -125,7 +103,7 @@ public:
k_param_channel_roll = 170,
k_param_channel_pitch,
k_param_channel_throttle,
k_param_channel_yaw,
k_param_channel_rudder,
k_param_rc_5,
k_param_rc_6,
k_param_rc_7,
@ -182,13 +160,13 @@ public:
// heading error from command to roll command deviation from trim
// (bank to turn strategy)
//
k_param_heading_to_roll_PID = 240,
k_param_pidNavRoll = 240,
// Roll-to-servo PID:
// roll error from command to roll servo deviation from trim
// (tracks commanded bank angle)
//
k_param_roll_to_servo_PID,
k_param_pidServoRoll,
//
// Pitch control
@ -197,13 +175,13 @@ public:
// pitch error from command to pitch servo deviation from trim
// (front-side strategy)
//
k_param_pitch_to_servo_PID,
k_param_pidServoPitch,
// Airspeed-to-pitch PID:
// airspeed error from command to pitch servo deviation from trim
// (back-side strategy)
//
k_param_airspeed_to_pitch_PID,
k_param_pidNavPitchAirspeed,
//
// Yaw control
@ -212,7 +190,7 @@ public:
// yaw rate error from command to yaw servo deviation from trim
// (stabilizes dutch roll)
//
k_param_yaw_to_servo_PID,
k_param_pidServoRudder,
//
// Throttle control
@ -221,13 +199,13 @@ public:
// total energy error from command to throttle servo deviation from trim
// (throttle back-side strategy alternative)
//
k_param_energy_to_throttle_PID,
k_param_pidTeThrottle,
// Altitude-to-pitch PID:
// altitude error from command to pitch servo deviation from trim
// (throttle front-side strategy alternative)
//
k_param_altitude_to_pitch_PID,
k_param_pidNavPitchAltitude,
// 254,255: reserved
};
@ -363,131 +341,109 @@ public:
PID pidTeThrottle;
PID pidNavPitchAltitude;
uint8_t junk;
// Note: keep initializers here in the same order as they are declared above.
Parameters() :
// variable default key name
//-------------------------------------------------------------------------------------------------------
format_version (k_format_version, k_param_format_version, PSTR("SYSID_SW_MREV")),
software_type (k_software_type, k_param_software_type, PSTR("SYSID_SW_TYPE")),
format_version (k_format_version),
software_type (k_software_type),
sysid_this_mav (MAV_SYSTEM_ID, k_param_sysid_this_mav, PSTR("SYSID_THISMAV")),
sysid_my_gcs (255, k_param_sysid_my_gcs, PSTR("SYSID_MYGCS")),
serial3_baud (SERIAL3_BAUD/1000, k_param_serial3_baud, PSTR("SERIAL3_BAUD")),
sysid_this_mav (MAV_SYSTEM_ID),
sysid_my_gcs (255),
serial3_baud (SERIAL3_BAUD/1000),
kff_pitch_compensation (PITCH_COMP, k_param_kff_pitch_compensation, PSTR("KFF_PTCHCOMP")),
kff_rudder_mix (RUDDER_MIX, k_param_kff_rudder_mix, PSTR("KFF_RDDRMIX")),
kff_pitch_to_throttle (P_TO_T, k_param_kff_pitch_to_throttle, PSTR("KFF_PTCH2THR")),
kff_throttle_to_pitch (T_TO_P, k_param_kff_throttle_to_pitch, PSTR("KFF_THR2PTCH")),
kff_pitch_compensation (PITCH_COMP),
kff_rudder_mix (RUDDER_MIX),
kff_pitch_to_throttle (P_TO_T),
kff_throttle_to_pitch (T_TO_P),
crosstrack_gain (XTRACK_GAIN_SCALED, k_param_crosstrack_gain, PSTR("XTRK_GAIN_SC")),
crosstrack_entry_angle (XTRACK_ENTRY_ANGLE_CENTIDEGREE,k_param_crosstrack_entry_angle, PSTR("XTRK_ANGLE_CD")),
crosstrack_gain (XTRACK_GAIN_SCALED),
crosstrack_entry_angle (XTRACK_ENTRY_ANGLE_CENTIDEGREE),
altitude_mix (ALTITUDE_MIX, k_param_altitude_mix, PSTR("ALT_MIX")),
airspeed_ratio (AIRSPEED_RATIO, k_param_airspeed_ratio, PSTR("ARSPD_RATIO")),
airspeed_offset (0, k_param_airspeed_offset, PSTR("ARSPD_OFFSET")),
altitude_mix (ALTITUDE_MIX),
airspeed_ratio (AIRSPEED_RATIO),
airspeed_offset (0),
/* XXX waypoint_mode missing here */
command_total (0, k_param_command_total, PSTR("CMD_TOTAL")),
command_index (0, k_param_command_index, PSTR("CMD_INDEX")),
waypoint_radius (WP_RADIUS_DEFAULT, k_param_waypoint_radius, PSTR("WP_RADIUS")),
loiter_radius (LOITER_RADIUS_DEFAULT, k_param_loiter_radius, PSTR("WP_LOITER_RAD")),
command_total (0),
command_index (0),
waypoint_radius (WP_RADIUS_DEFAULT),
loiter_radius (LOITER_RADIUS_DEFAULT),
#if GEOFENCE_ENABLED == ENABLED
fence_action (0, k_param_fence_action, PSTR("FENCE_ACTION")),
fence_total (0, k_param_fence_total, PSTR("FENCE_TOTAL")),
fence_channel (0, k_param_fence_channel, PSTR("FENCE_CHANNEL")),
fence_minalt (0, k_param_fence_minalt, PSTR("FENCE_MINALT")),
fence_maxalt (0, k_param_fence_maxalt, PSTR("FENCE_MAXALT")),
fence_action (0),
fence_total (0),
fence_channel (0),
fence_minalt (0),
fence_maxalt (0),
#endif
flybywire_airspeed_min (AIRSPEED_FBW_MIN, k_param_flybywire_airspeed_min, PSTR("ARSPD_FBW_MIN")),
flybywire_airspeed_max (AIRSPEED_FBW_MAX, k_param_flybywire_airspeed_max, PSTR("ARSPD_FBW_MAX")),
flybywire_airspeed_min (AIRSPEED_FBW_MIN),
flybywire_airspeed_max (AIRSPEED_FBW_MAX),
throttle_min (THROTTLE_MIN, k_param_throttle_min, PSTR("THR_MIN")),
throttle_max (THROTTLE_MAX, k_param_throttle_max, PSTR("THR_MAX")),
throttle_slewrate (THROTTLE_SLEW_LIMIT, k_param_throttle_slewrate, PSTR("THR_SLEWRATE")),
throttle_fs_enabled (THROTTLE_FAILSAFE, k_param_throttle_fs_enabled, PSTR("THR_FAILSAFE")),
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")),
throttle_min (THROTTLE_MIN),
throttle_max (THROTTLE_MAX),
throttle_slewrate (THROTTLE_SLEW_LIMIT),
throttle_fs_enabled (THROTTLE_FAILSAFE),
throttle_fs_value (THROTTLE_FS_VALUE),
throttle_cruise (THROTTLE_CRUISE),
short_fs_action (SHORT_FAILSAFE_ACTION, k_param_short_fs_action, PSTR("FS_SHORT_ACTN")),
long_fs_action (LONG_FAILSAFE_ACTION, k_param_long_fs_action, PSTR("FS_LONG_ACTN")),
gcs_heartbeat_fs_enabled(GCS_HEARTBEAT_FAILSAFE, k_param_gcs_heartbeat_fs_enabled, PSTR("FS_GCS_ENABL")),
short_fs_action (SHORT_FAILSAFE_ACTION),
long_fs_action (LONG_FAILSAFE_ACTION),
gcs_heartbeat_fs_enabled(GCS_HEARTBEAT_FAILSAFE),
flight_mode_channel (FLIGHT_MODE_CHANNEL, k_param_flight_mode_channel, PSTR("FLTMODE_CH")),
flight_mode1 (FLIGHT_MODE_1, k_param_flight_mode1, PSTR("FLTMODE1")),
flight_mode2 (FLIGHT_MODE_2, k_param_flight_mode2, PSTR("FLTMODE2")),
flight_mode3 (FLIGHT_MODE_3, k_param_flight_mode3, PSTR("FLTMODE3")),
flight_mode4 (FLIGHT_MODE_4, k_param_flight_mode4, PSTR("FLTMODE4")),
flight_mode5 (FLIGHT_MODE_5, k_param_flight_mode5, PSTR("FLTMODE5")),
flight_mode6 (FLIGHT_MODE_6, k_param_flight_mode6, PSTR("FLTMODE6")),
flight_mode_channel (FLIGHT_MODE_CHANNEL),
flight_mode1 (FLIGHT_MODE_1),
flight_mode2 (FLIGHT_MODE_2),
flight_mode3 (FLIGHT_MODE_3),
flight_mode4 (FLIGHT_MODE_4),
flight_mode5 (FLIGHT_MODE_5),
flight_mode6 (FLIGHT_MODE_6),
roll_limit (HEAD_MAX_CENTIDEGREE, k_param_roll_limit, PSTR("LIM_ROLL_CD")),
pitch_limit_max (PITCH_MAX_CENTIDEGREE, k_param_pitch_limit_max, PSTR("LIM_PITCH_MAX")),
pitch_limit_min (PITCH_MIN_CENTIDEGREE, k_param_pitch_limit_min, PSTR("LIM_PITCH_MIN")),
roll_limit (HEAD_MAX_CENTIDEGREE),
pitch_limit_max (PITCH_MAX_CENTIDEGREE),
pitch_limit_min (PITCH_MIN_CENTIDEGREE),
auto_trim (AUTO_TRIM, k_param_auto_trim, PSTR("TRIM_AUTO")),
switch_enable (REVERSE_SWITCH, k_param_switch_enable, PSTR("SWITCH_ENABLE")),
mix_mode (ELEVON_MIXING, k_param_mix_mode, PSTR("ELEVON_MIXING")),
reverse_elevons (ELEVON_REVERSE, k_param_reverse_elevons, PSTR("ELEVON_REVERSE")),
reverse_ch1_elevon (ELEVON_CH1_REVERSE, k_param_reverse_ch1_elevon, PSTR("ELEVON_CH1_REV")),
reverse_ch2_elevon (ELEVON_CH2_REVERSE, k_param_reverse_ch2_elevon, PSTR("ELEVON_CH2_REV")),
num_resets (0, k_param_num_resets, PSTR("SYS_NUM_RESETS")),
log_bitmask (DEFAULT_LOG_BITMASK, k_param_log_bitmask, PSTR("LOG_BITMASK")),
log_last_filenumber (0, k_param_log_last_filenumber, PSTR("LOG_LASTFILE")),
reset_switch_chan (0, k_param_reset_switch_chan, PSTR("RST_SWITCH_CH")),
airspeed_cruise (AIRSPEED_CRUISE_CM, k_param_airspeed_cruise, PSTR("TRIM_ARSPD_CM")),
min_gndspeed (MIN_GNDSPEED_CM, k_param_min_gndspeed, PSTR("MIN_GNDSPD_CM")),
pitch_trim (0, k_param_pitch_trim, PSTR("TRIM_PITCH_CD")),
RTL_altitude (ALT_HOLD_HOME_CM, k_param_RTL_altitude, PSTR("ALT_HOLD_RTL")),
FBWB_min_altitude (ALT_HOLD_FBW_CM, k_param_FBWB_min_altitude, PSTR("ALT_HOLD_FBWCM")),
ground_temperature (0, k_param_ground_temperature, PSTR("GND_TEMP")),
ground_pressure (0, k_param_ground_pressure, PSTR("GND_ABS_PRESS")),
compass_enabled (MAGNETOMETER, k_param_compass_enabled, PSTR("MAG_ENABLE")),
flap_1_percent (FLAP_1_PERCENT, k_param_flap_1_percent, PSTR("FLAP_1_PERCNT")),
flap_1_speed (FLAP_1_SPEED, k_param_flap_1_speed, PSTR("FLAP_1_SPEED")),
flap_2_percent (FLAP_2_PERCENT, k_param_flap_2_percent, PSTR("FLAP_2_PERCNT")),
flap_2_speed (FLAP_2_SPEED, k_param_flap_2_speed, PSTR("FLAP_2_SPEED")),
auto_trim (AUTO_TRIM),
switch_enable (REVERSE_SWITCH),
mix_mode (ELEVON_MIXING),
reverse_elevons (ELEVON_REVERSE),
reverse_ch1_elevon (ELEVON_CH1_REVERSE),
reverse_ch2_elevon (ELEVON_CH2_REVERSE),
num_resets (0),
log_bitmask (DEFAULT_LOG_BITMASK),
log_last_filenumber (0),
reset_switch_chan (0),
airspeed_cruise (AIRSPEED_CRUISE_CM),
min_gndspeed (MIN_GNDSPEED_CM),
pitch_trim (0),
RTL_altitude (ALT_HOLD_HOME_CM),
FBWB_min_altitude (ALT_HOLD_FBW_CM),
ground_temperature (0),
ground_pressure (0),
compass_enabled (MAGNETOMETER),
flap_1_percent (FLAP_1_PERCENT),
flap_1_speed (FLAP_1_SPEED),
flap_2_percent (FLAP_2_PERCENT),
flap_2_speed (FLAP_2_SPEED),
battery_monitoring (DISABLED, k_param_battery_monitoring, PSTR("BATT_MONITOR")),
volt_div_ratio (VOLT_DIV_RATIO, k_param_volt_div_ratio, PSTR("VOLT_DIVIDER")),
curr_amp_per_volt (CURR_AMP_PER_VOLT, k_param_curr_amp_per_volt, PSTR("AMP_PER_VOLT")),
input_voltage (INPUT_VOLTAGE, k_param_input_voltage, PSTR("INPUT_VOLTS")),
pack_capacity (HIGH_DISCHARGE, k_param_pack_capacity, PSTR("BATT_CAPACITY")),
inverted_flight_ch (0, k_param_inverted_flight_ch, PSTR("INVERTEDFLT_CH")),
sonar_enabled (SONAR_ENABLED, k_param_sonar_enabled, PSTR("SONAR_ENABLE")),
airspeed_enabled (AIRSPEED_SENSOR, k_param_airspeed_enabled, PSTR("ARSPD_ENABLE")),
battery_monitoring (DISABLED),
volt_div_ratio (VOLT_DIV_RATIO),
curr_amp_per_volt (CURR_AMP_PER_VOLT),
input_voltage (INPUT_VOLTAGE),
pack_capacity (HIGH_DISCHARGE),
inverted_flight_ch (0),
sonar_enabled (SONAR_ENABLED),
airspeed_enabled (AIRSPEED_SENSOR),
// Note - total parameter name length must be less than 14 characters for MAVLink compatibility!
// RC channel group key name
//----------------------------------------------------------------------
channel_roll (k_param_channel_roll, PSTR("RC1_")),
channel_pitch (k_param_channel_pitch, PSTR("RC2_")),
channel_throttle (k_param_channel_throttle, PSTR("RC3_")),
channel_rudder (k_param_channel_yaw, PSTR("RC4_")),
rc_5 (k_param_rc_5, PSTR("RC5_")),
rc_6 (k_param_rc_6, PSTR("RC6_")),
rc_7 (k_param_rc_7, PSTR("RC7_")),
rc_8 (k_param_rc_8, PSTR("RC8_")),
// PID controller group key name initial P initial I initial D initial imax
//---------------------------------------------------------------------------------------------------------------------------------------
pidNavRoll (k_param_heading_to_roll_PID, PSTR("HDNG2RLL_"), NAV_ROLL_P, NAV_ROLL_I, NAV_ROLL_D, NAV_ROLL_INT_MAX_CENTIDEGREE),
pidServoRoll (k_param_roll_to_servo_PID, PSTR("RLL2SRV_"), SERVO_ROLL_P, SERVO_ROLL_I, SERVO_ROLL_D, SERVO_ROLL_INT_MAX_CENTIDEGREE),
pidServoPitch (k_param_pitch_to_servo_PID, PSTR("PTCH2SRV_"), SERVO_PITCH_P, SERVO_PITCH_I, SERVO_PITCH_D, SERVO_PITCH_INT_MAX_CENTIDEGREE),
pidNavPitchAirspeed (k_param_airspeed_to_pitch_PID, PSTR("ARSP2PTCH_"), NAV_PITCH_ASP_P, NAV_PITCH_ASP_I, NAV_PITCH_ASP_D, NAV_PITCH_ASP_INT_MAX_CMSEC),
pidServoRudder (k_param_yaw_to_servo_PID, PSTR("YW2SRV_"), SERVO_YAW_P, SERVO_YAW_I, SERVO_YAW_D, SERVO_YAW_INT_MAX),
pidTeThrottle (k_param_energy_to_throttle_PID, PSTR("ENRGY2THR_"), THROTTLE_TE_P, THROTTLE_TE_I, THROTTLE_TE_D, THROTTLE_TE_INT_MAX),
pidNavPitchAltitude (k_param_altitude_to_pitch_PID, PSTR("ALT2PTCH_"), NAV_PITCH_ALT_P, NAV_PITCH_ALT_I, NAV_PITCH_ALT_D, NAV_PITCH_ALT_INT_MAX_CM),
junk(0) // XXX just so that we can add things without worrying about the trailing comma
{
}
// PID controller initial P initial I initial D initial imax
//-----------------------------------------------------------------------------------
pidNavRoll (NAV_ROLL_P, NAV_ROLL_I, NAV_ROLL_D, NAV_ROLL_INT_MAX_CENTIDEGREE),
pidServoRoll (SERVO_ROLL_P, SERVO_ROLL_I, SERVO_ROLL_D, SERVO_ROLL_INT_MAX_CENTIDEGREE),
pidServoPitch (SERVO_PITCH_P, SERVO_PITCH_I, SERVO_PITCH_D, SERVO_PITCH_INT_MAX_CENTIDEGREE),
pidNavPitchAirspeed (NAV_PITCH_ASP_P, NAV_PITCH_ASP_I, NAV_PITCH_ASP_D, NAV_PITCH_ASP_INT_MAX_CMSEC),
pidServoRudder (SERVO_YAW_P, SERVO_YAW_I, SERVO_YAW_D, SERVO_YAW_INT_MAX),
pidTeThrottle (THROTTLE_TE_P, THROTTLE_TE_I, THROTTLE_TE_D, THROTTLE_TE_INT_MAX),
pidNavPitchAltitude (NAV_PITCH_ALT_P, NAV_PITCH_ALT_I, NAV_PITCH_ALT_D, NAV_PITCH_ALT_INT_MAX_CM)
{}
};
#endif // PARAMETERS_H

159
ArduPlane/Parameters.pde Normal file
View File

@ -0,0 +1,159 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
/*
ArduPlane parameter definitions
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.
*/
#define GSCALAR(v, name) { g.v.vtype, name, Parameters::k_param_ ## v, &g.v }
#define GGROUP(v, name, class) { AP_PARAM_GROUP, name, Parameters::k_param_ ## v, &g.v, class::var_info }
#define GOBJECT(v, name, class) { AP_PARAM_GROUP, name, Parameters::k_param_ ## v, &v, class::var_info }
static const AP_Param::Info var_info[] PROGMEM = {
GSCALAR(format_version, "FORMAT_VERSION"),
GSCALAR(software_type, "SYSID_SW_TYPE"),
GSCALAR(sysid_this_mav, "SYSID_THISMAV"),
GSCALAR(sysid_my_gcs, "SYSID_MYGCS"),
GSCALAR(serial3_baud, "SERIAL3_BAUD"),
GSCALAR(kff_pitch_compensation, "KFF_PTCHCOMP"),
GSCALAR(kff_rudder_mix, "KFF_RDDRMIX"),
GSCALAR(kff_pitch_to_throttle, "KFF_PTCH2THR"),
GSCALAR(kff_throttle_to_pitch, "KFF_THR2PTCH"),
GSCALAR(crosstrack_gain, "XTRK_GAIN_SC"),
GSCALAR(crosstrack_entry_angle, "XTRK_ANGLE_CD"),
GSCALAR(altitude_mix, "ALT_MIX"),
GSCALAR(airspeed_ratio, "ARSPD_RATIO"),
GSCALAR(airspeed_offset, "ARSPD_OFFSET"),
GSCALAR(command_total, "CMD_TOTAL"),
GSCALAR(command_index, "CMD_INDEX"),
GSCALAR(waypoint_radius, "WP_RADIUS"),
GSCALAR(loiter_radius, "WP_LOITER_RAD"),
#if GEOFENCE_ENABLED == ENABLED
GSCALAR(fence_action, "FENCE_ACTION"),
GSCALAR(fence_total, "FENCE_TOTAL"),
GSCALAR(fence_channel, "FENCE_CHANNEL"),
GSCALAR(fence_minalt, "FENCE_MINALT"),
GSCALAR(fence_maxalt, "FENCE_MAXALT"),
#endif
GSCALAR(flybywire_airspeed_min, "ARSPD_FBW_MIN"),
GSCALAR(flybywire_airspeed_max, "ARSPD_FBW_MAX"),
GSCALAR(throttle_min, "THR_MIN"),
GSCALAR(throttle_max, "THR_MAX"),
GSCALAR(throttle_slewrate, "THR_SLEWRATE"),
GSCALAR(throttle_fs_enabled, "THR_FAILSAFE"),
GSCALAR(throttle_fs_value, "THR_FS_VALUE"),
GSCALAR(throttle_cruise, "TRIM_THROTTLE"),
GSCALAR(short_fs_action, "FS_SHORT_ACTN"),
GSCALAR(long_fs_action, "FS_LONG_ACTN"),
GSCALAR(gcs_heartbeat_fs_enabled, "FS_GCS_ENABL"),
GSCALAR(flight_mode_channel, "FLTMODE_CH"),
GSCALAR(flight_mode1, "FLTMODE1"),
GSCALAR(flight_mode2, "FLTMODE2"),
GSCALAR(flight_mode3, "FLTMODE3"),
GSCALAR(flight_mode4, "FLTMODE4"),
GSCALAR(flight_mode5, "FLTMODE5"),
GSCALAR(flight_mode6, "FLTMODE6"),
GSCALAR(roll_limit, "LIM_ROLL_CD"),
GSCALAR(pitch_limit_max, "LIM_PITCH_MAX"),
GSCALAR(pitch_limit_min, "LIM_PITCH_MIN"),
GSCALAR(auto_trim, "TRIM_AUTO"),
GSCALAR(switch_enable, "SWITCH_ENABLE"),
GSCALAR(mix_mode, "ELEVON_MIXING"),
GSCALAR(reverse_elevons, "ELEVON_REVERSE"),
GSCALAR(reverse_ch1_elevon, "ELEVON_CH1_REV"),
GSCALAR(reverse_ch2_elevon, "ELEVON_CH2_REV"),
GSCALAR(num_resets, "SYS_NUM_RESETS"),
GSCALAR(log_bitmask, "LOG_BITMASK"),
GSCALAR(log_last_filenumber, "LOG_LASTFILE"),
GSCALAR(reset_switch_chan, "RST_SWITCH_CH"),
GSCALAR(airspeed_cruise, "TRIM_ARSPD_CM"),
GSCALAR(min_gndspeed, "MIN_GNDSPD_CM"),
GSCALAR(pitch_trim, "TRIM_PITCH_CD"),
GSCALAR(RTL_altitude, "ALT_HOLD_RTL"),
GSCALAR(FBWB_min_altitude, "ALT_HOLD_FBWCM"),
GSCALAR(ground_temperature, "GND_TEMP"),
GSCALAR(ground_pressure, "GND_ABS_PRESS"),
GSCALAR(compass_enabled, "MAG_ENABLE"),
GSCALAR(flap_1_percent, "FLAP_1_PERCNT"),
GSCALAR(flap_1_speed, "FLAP_1_SPEED"),
GSCALAR(flap_2_percent, "FLAP_2_PERCNT"),
GSCALAR(flap_2_speed, "FLAP_2_SPEED"),
GSCALAR(battery_monitoring, "BATT_MONITOR"),
GSCALAR(volt_div_ratio, "VOLT_DIVIDER"),
GSCALAR(curr_amp_per_volt, "AMP_PER_VOLT"),
GSCALAR(input_voltage, "INPUT_VOLTS"),
GSCALAR(pack_capacity, "BATT_CAPACITY"),
GSCALAR(inverted_flight_ch, "INVERTEDFLT_CH"),
GSCALAR(sonar_enabled, "SONAR_ENABLE"),
GSCALAR(airspeed_enabled, "ARSPD_ENABLE"),
GGROUP(channel_roll, "RC1_", RC_Channel),
GGROUP(channel_pitch, "RC2_", RC_Channel),
GGROUP(channel_throttle, "RC3_", RC_Channel),
GGROUP(channel_rudder, "RC4_", RC_Channel),
GGROUP(rc_5, "RC5_", RC_Channel_aux),
GGROUP(rc_6, "RC6_", RC_Channel_aux),
GGROUP(rc_7, "RC7_", RC_Channel_aux),
GGROUP(rc_8, "RC8_", RC_Channel_aux),
GGROUP(pidNavRoll, "HDNG2RLL_", PID),
GGROUP(pidServoRoll, "RLL2SRV_", PID),
GGROUP(pidServoPitch, "PTCH2SRV_", PID),
GGROUP(pidNavPitchAirspeed, "ARSP2PTCH_", PID),
GGROUP(pidServoRudder, "YW2SRV_", PID),
GGROUP(pidTeThrottle, "ENRGY2THR_", PID),
GGROUP(pidNavPitchAltitude, "ALT2PTCH_", PID),
// variables not in the g class which contain EEPROM saved variables
GOBJECT(compass, "COMPASS_", Compass),
GOBJECT(gcs0, "SR0_", GCS_MAVLINK),
GOBJECT(gcs3, "SR3_", GCS_MAVLINK)
};
static void load_parameters(void)
{
// setup the AP_Var subsystem for storage to EEPROM
if (!AP_Param::setup(var_info, sizeof(var_info)/sizeof(var_info[0]), WP_START_BYTE)) {
// this can only happen on startup, and its a definate coding
// error. Best not to continue so the programmer catches it
while (1) {
Serial.println_P(PSTR("ERROR: Failed to setup AP_Param"));
delay(1000);
}
}
if (!g.format_version.load() ||
g.format_version != Parameters::k_format_version) {
// erase all parameters
Serial.printf_P(PSTR("Firmware change: erasing EEPROM...\n"));
AP_Param::erase_all();
// save the current format version
g.format_version.set_and_save(Parameters::k_format_version);
Serial.println_P(PSTR("done."));
} else {
unsigned long before = micros();
// Load all auto-loaded EEPROM variables
AP_Param::load_all();
Serial.printf_P(PSTR("load_all took %luus\n"), micros() - before);
}
}

5
ArduPlane/climb_rate.pde
View File

@ -1,5 +1,7 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#if 0 // currently unused
struct DataPoint {
unsigned long x;
long y;
@ -17,8 +19,7 @@ long yi;
long xiyi;
unsigned long xi2;
#if 0 // currently unused
static void add_altitude_data(unsigned long xl, long y)
void add_altitude_data(unsigned long xl, long y)
{
//Reset the regression if our X variable overflowed
if (xl < xoffset)

25
ArduPlane/commands.pde
View File

@ -1,19 +1,7 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
/* Functions in this file:
void init_commands()
void update_auto()
void reload_commands_airstart()
struct Location get_cmd_with_index(int i)
void set_cmd_with_index(struct Location temp, int i)
void increment_cmd_index()
void decrement_cmd_index()
long read_alt_to_hold()
void set_next_WP(struct Location *wp)
void set_guided_WP(void)
void init_home()
************************************************************
*/
/*
logic for dealing with the current command in the mission and home location
*/
static void init_commands()
{
@ -125,13 +113,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) {

38
ArduPlane/commands_logic.pde
View File

@ -276,7 +276,7 @@ static void do_loiter_time()
static bool verify_takeoff()
{
if (g_gps->ground_speed > 300){
if(hold_course == -1){
if (hold_course == -1) {
// save our current course to take off
if(g.compass_enabled) {
hold_course = dcm.yaw_sensor;
@ -286,7 +286,7 @@ static bool verify_takeoff()
}
}
if(hold_course > -1){
if (hold_course != -1) {
// recalc bearing error with hold_course;
nav_bearing = hold_course;
// recalc bearing error
@ -302,23 +302,35 @@ static bool verify_takeoff()
}
}
// we are executing a landing
static bool verify_land()
{
// we don't verify landing - we never go to a new Nav command after Land
// we don't 'verify' landing in the sense that it never completes,
// so we don't verify command completion. Instead we use this to
// adjust final landing parameters
// Set land_complete if we are within 2 seconds distance or within
// 3 meters altitude of the landing point
if (((wp_distance > 0) && (wp_distance <= (2*g_gps->ground_speed/100)))
|| (current_loc.alt <= next_WP.alt + 300)){
land_complete = 1; //Set land_complete if we are within 2 seconds distance or within 3 meters altitude
land_complete = 1;
if(hold_course == -1){
// save our current course to land
//hold_course = yaw_sensor;
// save the course line of the runway to land
hold_course = crosstrack_bearing;
if(hold_course == -1) {
// we have just reached the threshold of 2 seconds or 3
// meters to landing. We now don't want to do any radical
// turns, as rolling could put the wings into the runway.
// To prevent further turns we set hold_course to the
// current heading. Previously we set this to
// crosstrack_bearing, but the xtrack bearing can easily
// be quite large at this point, and that could induce a
// sudden large roll correction which is very nasty at
// this point in the landing.
hold_course = dcm.yaw_sensor;
}
}
if(hold_course > -1){
if (hold_course != -1){
// recalc bearing error with hold_course;
nav_bearing = hold_course;
// recalc bearing error
@ -489,15 +501,15 @@ static void do_change_speed()
{
case 0: // Airspeed
if(next_nonnav_command.alt > 0)
g.airspeed_cruise.set_and_save(next_nonnav_command.alt * 100);
g.airspeed_cruise.set(next_nonnav_command.alt * 100);
break;
case 1: // Ground speed
g.min_gndspeed.set_and_save(next_nonnav_command.alt * 100);
g.min_gndspeed.set(next_nonnav_command.alt * 100);
break;
}
if(next_nonnav_command.lat > 0)
g.throttle_cruise.set_and_save(next_nonnav_command.lat);
g.throttle_cruise.set(next_nonnav_command.lat);
}
static void do_set_home()

1
ArduPlane/config.h
View File

@ -51,7 +51,6 @@
#endif
#if defined( __AVR_ATmega1280__ )
#define CLI_ENABLED DISABLED
#define LOGGING_ENABLED DISABLED
#endif

2
ArduPlane/defines.h
View File

@ -200,7 +200,7 @@ enum gcs_severity {
// EEPROM addresses
#define EEPROM_MAX_ADDR 4096
// parameters get the first 1KiB of EEPROM, remainder is for waypoints
// parameters get the first 1024 bytes of EEPROM, remainder is for waypoints
#define WP_START_BYTE 0x400 // where in memory home WP is stored + all other WP
#define WP_SIZE 15

2
ArduPlane/events.pde
View File

@ -79,7 +79,7 @@ static void failsafe_short_off_event()
}
#if BATTERY_EVENT == ENABLED
static void low_battery_event(void)
void low_battery_event(void)
{
gcs_send_text_P(SEVERITY_HIGH,PSTR("Low Battery!"));
set_mode(RTL);

5
ArduPlane/setup.pde
View File

@ -65,7 +65,8 @@ setup_show(uint8_t argc, const Menu::arg *argv)
Serial.printf_P(PSTR("Raw Values\n"));
print_divider();
AP_Var_menu_show(argc, argv);
AP_Param::show_all();
return(0);
}
@ -85,7 +86,7 @@ setup_factory(uint8_t argc, const Menu::arg *argv)
if (('y' != c) && ('Y' != c))
return(-1);
AP_Var::erase_all();
AP_Param::erase_all();
Serial.printf_P(PSTR("\nFACTORY RESET complete - please reset APM to continue"));
//default_flight_modes(); // This will not work here. Replacement code located in init_ardupilot()

23
ArduPlane/system.pde
View File

@ -128,28 +128,7 @@ static void init_ardupilot()
//
// Check the EEPROM format version before loading any parameters from EEPROM.
//
if (!g.format_version.load() ||
g.format_version != Parameters::k_format_version) {
// erase all parameters
Serial.printf_P(PSTR("Firmware change: erasing EEPROM...\n"));
delay(100); // wait for serial send
AP_Var::erase_all();
// save the current format version
g.format_version.set_and_save(Parameters::k_format_version);
Serial.println_P(PSTR("done."));
} else {
unsigned long before = micros();
// Load all auto-loaded EEPROM variables
AP_Var::load_all();
Serial.printf_P(PSTR("load_all took %luus\n"), micros() - before);
Serial.printf_P(PSTR("using %u bytes of memory (%u resets)\n"),
AP_Var::get_memory_use(), (unsigned)g.num_resets);
}
load_parameters();
// keep a record of how many resets have happened. This can be
// used to detect in-flight resets

6
ArduPlane/test.pde
View File

@ -611,9 +611,9 @@ test_mag(uint8_t argc, const Menu::arg *argv)
Vector3f maggy = compass.get_offsets();
Serial.printf_P(PSTR("Heading: %ld, XYZ: %d, %d, %d,\tXYZoff: %6.2f, %6.2f, %6.2f\n"),
(wrap_360(ToDeg(compass.heading) * 100)) /100,
compass.mag_x,
compass.mag_y,
compass.mag_z,
(int)compass.mag_x,
(int)compass.mag_y,
(int)compass.mag_z,
maggy.x,
maggy.y,
maggy.z);

15
Tools/ArdupilotMegaPlanner/Common.cs
View File

@ -299,21 +299,6 @@ namespace ArdupilotMega
OF_LOITER = 10
}
int fixme;
public enum bitmask
{
None = 0,
sonar_enable = 1,
compass_enabled = 2,
optflow_enabled = 4,
super_simple = 8,
waypoint_mode = 16,
esc_calibrate = 32,
heli_ext_gyro_enabled = 64,
heli_servo_averaging = 128,
heli_servo_manual = 256,
}
public static void linearRegression()
{
double[] values = { 4.8, 4.8, 4.5, 3.9, 4.4, 3.6, 3.6, 2.9, 3.5, 3.0, 2.5, 2.2, 2.6, 2.1, 2.2 };

8
Tools/ArdupilotMegaPlanner/Controls/AGauge.cs
View File

@ -221,13 +221,13 @@ namespace AGaugeApp
#region properties
[System.ComponentModel.Browsable(true)]
public Single Value0 { get { return m_value[0]; } set { m_NeedIdx = 0; Value = value; this.Invalidate(); } }
public Single Value0 { get { return m_value[0]; } set { m_NeedIdx = 0; m_value[0] = value; this.Invalidate(); } }
[System.ComponentModel.Browsable(true)]
public Single Value1 { get { return m_value[1]; } set { m_NeedIdx = 1; Value = value; this.Invalidate(); } }
public Single Value1 { get { return m_value[1]; } set { m_NeedIdx = 1; m_value[1] = value; this.Invalidate(); } }
[System.ComponentModel.Browsable(true)]
public Single Value2 { get { return m_value[2]; } set { m_NeedIdx = 2; Value = value; this.Invalidate(); } }
public Single Value2 { get { return m_value[2]; } set { m_NeedIdx = 2; m_value[2] = value; this.Invalidate(); } }
[System.ComponentModel.Browsable(true)]
public Single Value3 { get { return m_value[3]; } set { m_NeedIdx = 3; Value = value; this.Invalidate(); } }
public Single Value3 { get { return m_value[3]; } set { m_NeedIdx = 3; m_value[3] = value; this.Invalidate(); } }
[System.ComponentModel.Browsable(true),
System.ComponentModel.Category("AGauge"),

7
Tools/ArdupilotMegaPlanner/Controls/MyLabel.cs
View File

@ -24,7 +24,7 @@ namespace ArdupilotMega
public MyLabel()
{
}
public override string Text
{
get
@ -33,6 +33,7 @@ namespace ArdupilotMega
}
set
{
if (value == null)
return;
@ -46,8 +47,8 @@ namespace ArdupilotMega
noofchars = label.Length;
Size textSize = TextRenderer.MeasureText(value, this.Font);
this.Width = textSize.Width;
}
}
this.Invalidate();
}
}

65
Tools/ArdupilotMegaPlanner/CurrentState.cs
View File

@ -186,6 +186,59 @@ namespace ArdupilotMega
public ushort rcoverridech7 { get; set; }
public ushort rcoverridech8 { get; set; }
internal PointLatLngAlt HomeLocation = new PointLatLngAlt();
public float DistToMAV
{
get
{
// shrinking factor for longitude going to poles direction
double rads = Math.Abs(HomeLocation.Lat) * 0.0174532925;
double scaleLongDown = Math.Cos(rads);
double scaleLongUp = 1.0f / Math.Cos(rads);
//DST to Home
double dstlat = Math.Abs(HomeLocation.Lat - lat) * 111319.5;
double dstlon = Math.Abs(HomeLocation.Lng - lng) * 111319.5 * scaleLongDown;
return (float)Math.Sqrt((dstlat * dstlat) + (dstlon * dstlon));
}
}
public float ELToMAV
{
get
{
float dist = DistToMAV;
float altdiff = (float)(alt - HomeLocation.Alt);
float angle = (float)Math.Atan(altdiff / dist) * rad2deg;
return angle;
}
}
public float AZToMAV
{
get
{
// shrinking factor for longitude going to poles direction
double rads = Math.Abs(HomeLocation.Lat) * 0.0174532925;
double scaleLongDown = Math.Cos(rads);
double scaleLongUp = 1.0f / Math.Cos(rads);
//DIR to Home
double dstlon = (HomeLocation.Lng - lng); //OffSet_X
double dstlat = (HomeLocation.Lat - lat) * scaleLongUp; //OffSet Y
double bearing = 90 + (Math.Atan2(dstlat, -dstlon) * 57.295775); //absolut home direction
if (bearing < 0) bearing += 360;//normalization
//bearing = bearing - 180;//absolut return direction
//if (bearing < 0) bearing += 360;//normalization
return (float)bearing;
}
}
// current firmware
public MainV2.Firmwares firmware = MainV2.Firmwares.ArduPlane;
public float freemem { get; set; }
@ -658,6 +711,7 @@ namespace ArdupilotMega
groundspeed = gps.v;
groundcourse = gps.hdg;
//MAVLink.packets[MAVLink.MAVLINK_MSG_ID_GPS_RAW] = null;
}
#endif
@ -853,7 +907,16 @@ namespace ArdupilotMega
airspeed = vfr.airspeed;
alt = vfr.alt; // this might include baro
/*
if (vfr.throttle > 150 || vfr.throttle < 0)
{
Console.WriteLine(0);
}
else
{
Console.WriteLine(vfr.throttle);
}
*/
//climbrate = vfr.climb;
if ((DateTime.Now - lastalt).TotalSeconds >= 0.1 && oldalt != alt)

20
Tools/ArdupilotMegaPlanner/GCSViews/FlightData.cs
View File

@ -74,6 +74,9 @@ namespace ArdupilotMega.GCSViews
public static hud.HUD myhud = null;
public static GMapControl mymap = null;
PointLatLngAlt GuidedModeWP = new PointLatLngAlt();
AviWriter aviwriter;
public SplitContainer MainHcopy = null;
@ -440,7 +443,7 @@ namespace ArdupilotMega.GCSViews
{
if (plla == null || plla.Lng == 0 || plla.Lat == 0)
break;
addpolygonmarker(plla.Tag, plla.Lng, plla.Lat, (int)plla.Alt,plla.color);
addpolygonmarker(plla.Tag, plla.Lng, plla.Lat, (int)plla.Alt,plla.color,polygons);
}
RegeneratePolygon();
@ -448,6 +451,11 @@ namespace ArdupilotMega.GCSViews
waypoints = DateTime.Now;
}
if (MainV2.cs.mode.ToLower() == "guided" && GuidedModeWP != null && GuidedModeWP.Lat != 0)
{
addpolygonmarker("Guided Mode", GuidedModeWP.Lng, GuidedModeWP.Lat, (int)GuidedModeWP.Alt, Color.Blue, routes);
}
//routes.Polygons.Add(poly);
if (trackPoints.Count > 0)
@ -482,8 +490,6 @@ namespace ArdupilotMega.GCSViews
gMapControl1.Invalidate();
Application.DoEvents();
GC.Collect();
}
tracklast = DateTime.Now;
@ -533,7 +539,7 @@ namespace ArdupilotMega.GCSViews
});
}
private void addpolygonmarker(string tag, double lng, double lat, int alt, Color? color)
private void addpolygonmarker(string tag, double lng, double lat, int alt, Color? color, GMapOverlay overlay)
{
try
{
@ -559,8 +565,8 @@ namespace ArdupilotMega.GCSViews
}
}
polygons.Markers.Add(m);
polygons.Markers.Add(mBorders);
overlay.Markers.Add(m);
overlay.Markers.Add(mBorders);
}
catch (Exception) { }
}
@ -861,6 +867,8 @@ namespace ArdupilotMega.GCSViews
MainV2.comPort.setWP(gotohere, 0, MAVLink.MAV_FRAME.MAV_FRAME_GLOBAL_RELATIVE_ALT, (byte)2);
GuidedModeWP = new PointLatLngAlt(gotohere.lat, gotohere.lng, gotohere.alt,"Guided Mode");
MainV2.givecomport = false;
}
catch (Exception ex) { MainV2.givecomport = false; MessageBox.Show("Error sending command : " + ex.Message); }

15
Tools/ArdupilotMegaPlanner/GCSViews/FlightPlanner.cs
View File

@ -41,7 +41,6 @@ namespace ArdupilotMega.GCSViews
private Dictionary<string, string[]> cmdParamNames = new Dictionary<string, string[]>();
/// <summary>
/// Reads defines.h for all valid commands and eeprom positions
/// </summary>
@ -645,13 +644,13 @@ namespace ArdupilotMega.GCSViews
config(false);
if (MainV2.HomeLocation.Lat != 0 && MainV2.HomeLocation.Lng != 0)
if (MainV2.cs.HomeLocation.Lat != 0 && MainV2.cs.HomeLocation.Lng != 0)
{
TXT_homelat.Text = MainV2.HomeLocation.Lat.ToString();
TXT_homelat.Text = MainV2.cs.HomeLocation.Lat.ToString();
TXT_homelng.Text = MainV2.HomeLocation.Lng.ToString();
TXT_homelng.Text = MainV2.cs.HomeLocation.Lng.ToString();
TXT_homealt.Text = MainV2.HomeLocation.Alt.ToString();
TXT_homealt.Text = MainV2.cs.HomeLocation.Alt.ToString();
}
@ -1685,7 +1684,7 @@ namespace ArdupilotMega.GCSViews
sethome = false;
try
{
MainV2.HomeLocation.Lat = double.Parse(TXT_homelat.Text);
MainV2.cs.HomeLocation.Lat = double.Parse(TXT_homelat.Text);
}
catch { }
writeKML();
@ -1697,7 +1696,7 @@ namespace ArdupilotMega.GCSViews
sethome = false;
try
{
MainV2.HomeLocation.Lng = double.Parse(TXT_homelng.Text);
MainV2.cs.HomeLocation.Lng = double.Parse(TXT_homelng.Text);
}
catch { }
writeKML();
@ -1708,7 +1707,7 @@ namespace ArdupilotMega.GCSViews
sethome = false;
try
{
MainV2.HomeLocation.Alt = double.Parse(TXT_homealt.Text);
MainV2.cs.HomeLocation.Alt = double.Parse(TXT_homealt.Text);
}
catch { }
writeKML();

4
Tools/ArdupilotMegaPlanner/GCSViews/Simulation.cs
View File

@ -648,8 +648,8 @@ namespace ArdupilotMega.GCSViews
//JSBSimSEND.Client.Send(System.Text.Encoding.ASCII.GetBytes("set position/h-agl-ft 0\r\n"));
JSBSimSEND.Client.Send(System.Text.Encoding.ASCII.GetBytes("set position/lat-gc-deg " + MainV2.HomeLocation.Lat + "\r\n"));
JSBSimSEND.Client.Send(System.Text.Encoding.ASCII.GetBytes("set position/long-gc-deg " + MainV2.HomeLocation.Lng + "\r\n"));
JSBSimSEND.Client.Send(System.Text.Encoding.ASCII.GetBytes("set position/lat-gc-deg " + MainV2.cs.HomeLocation.Lat + "\r\n"));
JSBSimSEND.Client.Send(System.Text.Encoding.ASCII.GetBytes("set position/long-gc-deg " + MainV2.cs.HomeLocation.Lng + "\r\n"));
JSBSimSEND.Client.Send(System.Text.Encoding.ASCII.GetBytes("set attitude/phi-rad 0\r\n"));
JSBSimSEND.Client.Send(System.Text.Encoding.ASCII.GetBytes("set attitude/theta-rad 0\r\n"));

11
Tools/ArdupilotMegaPlanner/MainV2.cs
View File

@ -33,11 +33,6 @@ namespace ArdupilotMega
const int SW_SHOWNORMAL = 1;
const int SW_HIDE = 0;
/// <summary>
/// Home Location
/// </summary>
public static PointLatLngAlt HomeLocation = new PointLatLngAlt();
public static MAVLink comPort = new MAVLink();
public static string comportname = "";
public static Hashtable config = new Hashtable();
@ -208,13 +203,13 @@ namespace ArdupilotMega
try
{
if (config["TXT_homelat"] != null)
HomeLocation.Lat = double.Parse(config["TXT_homelat"].ToString());
cs.HomeLocation.Lat = double.Parse(config["TXT_homelat"].ToString());
if (config["TXT_homelng"] != null)
HomeLocation.Lng = double.Parse(config["TXT_homelng"].ToString());
cs.HomeLocation.Lng = double.Parse(config["TXT_homelng"].ToString());
if (config["TXT_homealt"] != null)
HomeLocation.Alt = double.Parse(config["TXT_homealt"].ToString());
cs.HomeLocation.Alt = double.Parse(config["TXT_homealt"].ToString());
}
catch { }

2
Tools/ArdupilotMegaPlanner/Properties/AssemblyInfo.cs
View File

@ -34,5 +34,5 @@ using System.Resources;
// by using the '*' as shown below:
// [assembly: AssemblyVersion("1.0.*")]
[assembly: AssemblyVersion("1.0.0.0")]
[assembly: AssemblyFileVersion("1.1.35")]
[assembly: AssemblyFileVersion("1.1.36")]
[assembly: NeutralResourcesLanguageAttribute("")]

2
Tools/ArdupilotMegaPlanner/Setup/Setup.cs
View File

@ -1401,6 +1401,8 @@ namespace ArdupilotMega.Setup
{
timer.Stop();
timer.Dispose();
tabControl1.SelectedIndex = 0;
}
private void CHK_enableoptflow_CheckedChanged(object sender, EventArgs e)

BIN
Tools/ArdupilotMegaPlanner/bin/Release/ArdupilotMegaPlanner.pdb
View File

Binary file not shown.

10
Tools/CPUInfo/output.txt
View File

@ -39,3 +39,13 @@ idiv32 37.71 usec/call
memcpy128 56.76 usec/call
memset128 49.71 usec/call
delay(1) 1005.14 usec/call
Additional notes:
eeprom_read_byte: 2 usec/call
eeprom_write_byte: first call costs 5 usec, each subsequent byte
costs 3480 usec as it waits for the EEPROM to be
ready from the previous byte
pgm_read_byte: 0.5 usec per byte

41
Tools/VARTest/APM_Config.h Normal file
View File

@ -0,0 +1,41 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
// This file is just a placeholder for your configuration file. If you wish to change any of the setup parameters from
// their default values, place the appropriate #define statements here.
// For example if you wanted the Port 3 baud rate to be 38400 you would add a statement like the one below (uncommented)
//#define SERIAL3_BAUD 38400
// # define CONFIG_APM_HARDWARE APM_HARDWARE_APM2
// # define APM2_BETA_HARDWARE
// You may also put an include statement here to point at another configuration file. This is convenient if you maintain
// different configuration files for different aircraft or HIL simulation. See the examples below
//#include "APM_Config_mavlink_hil.h"
//#include "Skywalker.h"
// The following are the recommended settings for Xplane simulation. Remove the leading "/* and trailing "*/" to enable:
/*
#define HIL_MODE HIL_MODE_ATTITUDE
*/
/*
// HIL_MODE SELECTION
//
// Mavlink supports
// 1. HIL_MODE_ATTITUDE : simulated position, airspeed, and attitude
// 2. HIL_MODE_SENSORS: full sensor simulation
#define HIL_MODE HIL_MODE_ATTITUDE
// Sensors
// All sensors are supported in all modes.
// The magnetometer is not used in
// HIL_MODE_ATTITUDE but you may leave it
// enabled if you wish.
#define AIRSPEED_SENSOR ENABLED
#define MAGNETOMETER ENABLED
#define AIRSPEED_CRUISE 25
#define THROTTLE_FAILSAFE ENABLED
*/

7
Tools/VARTest/Makefile Normal file
View File

@ -0,0 +1,7 @@
#
# Trivial makefile for building APM
#
include ../../libraries/AP_Common/Arduino.mk
sitl:
make -f ../../libraries/Desktop/Desktop.mk

449
Tools/VARTest/Parameters.h Normal file
View File

@ -0,0 +1,449 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#ifndef PARAMETERS_H
#define PARAMETERS_H
#include <AP_Common.h>
// Global parameter class.
//
class Parameters {
public:
// The version of the layout as described by the parameter enum.
//
// When changing the parameter enum in an incompatible fashion, this
// value should be incremented by one.
//
// The increment will prevent old parameters from being used incorrectly
// by newer code.
//
static const uint16_t k_format_version = 1;
// The parameter software_type is set up solely for ground station use
// and identifies the software type (eg ArduPilotMega versus ArduCopterMega)
// GCS will interpret values 0-9 as ArduPilotMega. Developers may use
// values within that range to identify different branches.
//
static const uint16_t k_software_type = 0; // 0 for APM trunk
enum {
// Layout version number, always key zero.
//
k_param_format_version = 0,
k_param_software_type,
// Misc
//
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,
k_param_reverse_ch2_elevon,
k_param_flap_1_percent,
k_param_flap_1_speed,
k_param_flap_2_percent,
k_param_flap_2_speed,
k_param_num_resets,
k_param_log_last_filenumber, // *** Deprecated - remove with next eeprom number change
k_param_reset_switch_chan,
// 110: Telemetry control
//
k_param_gcs0 = 110, // stream rates for port0
k_param_gcs3, // stream rates for port3
k_param_sysid_this_mav,
k_param_sysid_my_gcs,
k_param_serial3_baud,
// 120: Fly-by-wire control
//
k_param_flybywire_airspeed_min = 120,
k_param_flybywire_airspeed_max,
k_param_FBWB_min_altitude, // 0=disabled, minimum value for altitude in cm (for first time try 30 meters = 3000 cm)
//
// 130: Sensor parameters
//
k_param_IMU_calibration = 130,
k_param_altitude_mix,
k_param_airspeed_ratio,
k_param_ground_temperature,
k_param_ground_pressure,
k_param_compass_enabled,
k_param_compass,
k_param_battery_monitoring,
k_param_volt_div_ratio,
k_param_curr_amp_per_volt,
k_param_input_voltage,
k_param_pack_capacity,
k_param_airspeed_offset,
k_param_sonar_enabled,
k_param_airspeed_enabled,
//
// 150: Navigation parameters
//
k_param_crosstrack_gain = 150,
k_param_crosstrack_entry_angle,
k_param_roll_limit,
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,
//
// 170: Radio settings
//
k_param_channel_roll = 170,
k_param_channel_pitch,
k_param_channel_throttle,
k_param_channel_rudder,
k_param_rc_5,
k_param_rc_6,
k_param_rc_7,
k_param_rc_8,
k_param_throttle_min,
k_param_throttle_max,
k_param_throttle_fs_enabled,
k_param_throttle_fs_value,
k_param_throttle_cruise,
k_param_short_fs_action,
k_param_long_fs_action,
k_param_gcs_heartbeat_fs_enabled,
k_param_throttle_slewrate,
//
// 200: Feed-forward gains
//
k_param_kff_pitch_compensation = 200,
k_param_kff_rudder_mix,
k_param_kff_pitch_to_throttle,
k_param_kff_throttle_to_pitch,
//
// 210: flight modes
//
k_param_flight_mode_channel = 210,
k_param_flight_mode1,
k_param_flight_mode2,
k_param_flight_mode3,
k_param_flight_mode4,
k_param_flight_mode5,
k_param_flight_mode6,
//
// 220: Waypoint data
//
k_param_waypoint_mode = 220,
k_param_command_total,
k_param_command_index,
k_param_waypoint_radius,
k_param_loiter_radius,
k_param_fence_action,
k_param_fence_total,
k_param_fence_channel,
k_param_fence_minalt,
k_param_fence_maxalt,
//
// 240: PID Controllers
//
// Heading-to-roll PID:
// heading error from command to roll command deviation from trim
// (bank to turn strategy)
//
k_param_pidNavRoll = 240,
// Roll-to-servo PID:
// roll error from command to roll servo deviation from trim
// (tracks commanded bank angle)
//
k_param_pidServoRoll,
//
// Pitch control
//
// Pitch-to-servo PID:
// pitch error from command to pitch servo deviation from trim
// (front-side strategy)
//
k_param_pidServoPitch,
// Airspeed-to-pitch PID:
// airspeed error from command to pitch servo deviation from trim
// (back-side strategy)
//
k_param_pidNavPitchAirspeed,
//
// Yaw control
//
// Yaw-to-servo PID:
// yaw rate error from command to yaw servo deviation from trim
// (stabilizes dutch roll)
//
k_param_pidServoRudder,
//
// Throttle control
//
// Energy-to-throttle PID:
// total energy error from command to throttle servo deviation from trim
// (throttle back-side strategy alternative)
//
k_param_pidTeThrottle,
// Altitude-to-pitch PID:
// altitude error from command to pitch servo deviation from trim
// (throttle front-side strategy alternative)
//
k_param_pidNavPitchAltitude,
// 254,255: reserved
};
AP_Int16 format_version;
AP_Int8 software_type;
// Telemetry control
//
AP_Int16 sysid_this_mav;
AP_Int16 sysid_my_gcs;
AP_Int8 serial3_baud;
// Feed-forward gains
//
AP_Float kff_pitch_compensation;
AP_Float kff_rudder_mix;
AP_Float kff_pitch_to_throttle;
AP_Float kff_throttle_to_pitch;
// Crosstrack navigation
//
AP_Float crosstrack_gain;
AP_Int16 crosstrack_entry_angle;
// Estimation
//
AP_Float altitude_mix;
AP_Float airspeed_ratio;
AP_Int16 airspeed_offset;
// Waypoints
//
AP_Int8 waypoint_mode;
AP_Int8 command_total;
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
//
AP_Int8 flybywire_airspeed_min;
AP_Int8 flybywire_airspeed_max;
AP_Int16 FBWB_min_altitude;
// Throttle
//
AP_Int8 throttle_min;
AP_Int8 throttle_max;
AP_Int8 throttle_slewrate;
AP_Int8 throttle_fs_enabled;
AP_Int16 throttle_fs_value;
AP_Int8 throttle_cruise;
// Failsafe
AP_Int8 short_fs_action;
AP_Int8 long_fs_action;
AP_Int8 gcs_heartbeat_fs_enabled;
// Flight modes
//
AP_Int8 flight_mode_channel;
AP_Int8 flight_mode1;
AP_Int8 flight_mode2;
AP_Int8 flight_mode3;
AP_Int8 flight_mode4;
AP_Int8 flight_mode5;
AP_Int8 flight_mode6;
// Navigational maneuvering limits
//
AP_Int16 roll_limit;
AP_Int16 pitch_limit_max;
AP_Int16 pitch_limit_min;
// Misc
//
AP_Int8 auto_trim;
AP_Int8 switch_enable;
AP_Int8 mix_mode;
AP_Int8 reverse_elevons;
AP_Int8 reverse_ch1_elevon;
AP_Int8 reverse_ch2_elevon;
AP_Int16 num_resets;
AP_Int16 log_bitmask;
AP_Int16 log_last_filenumber; // *** Deprecated - remove with next eeprom number change
AP_Int8 reset_switch_chan;
AP_Int16 airspeed_cruise;
AP_Int16 min_gndspeed;
AP_Int16 pitch_trim;
AP_Int16 RTL_altitude;
AP_Int16 ground_temperature;
AP_Int32 ground_pressure;
AP_Int8 compass_enabled;
AP_Int16 angle_of_attack;
AP_Int8 battery_monitoring; // 0=disabled, 3=voltage only, 4=voltage and current
AP_Float volt_div_ratio;
AP_Float curr_amp_per_volt;
AP_Float input_voltage;
AP_Int16 pack_capacity; // Battery pack capacity less reserve
AP_Int8 inverted_flight_ch; // 0=disabled, 1-8 is channel for inverted flight trigger
AP_Int8 sonar_enabled;
AP_Int8 airspeed_enabled;
AP_Int8 flap_1_percent;
AP_Int8 flap_1_speed;
AP_Int8 flap_2_percent;
AP_Int8 flap_2_speed;
// RC channels
RC_Channel channel_roll;
RC_Channel channel_pitch;
RC_Channel channel_throttle;
RC_Channel channel_rudder;
RC_Channel_aux rc_5;
RC_Channel_aux rc_6;
RC_Channel_aux rc_7;
RC_Channel_aux rc_8;
// PID controllers
//
PID pidNavRoll;
PID pidServoRoll;
PID pidServoPitch;
PID pidNavPitchAirspeed;
PID pidServoRudder;
PID pidTeThrottle;
PID pidNavPitchAltitude;
Parameters() :
format_version (k_format_version),
software_type (k_software_type),
sysid_this_mav (MAV_SYSTEM_ID),
sysid_my_gcs (255),
serial3_baud (SERIAL3_BAUD/1000),
kff_pitch_compensation (PITCH_COMP),
kff_rudder_mix (RUDDER_MIX),
kff_pitch_to_throttle (P_TO_T),
kff_throttle_to_pitch (T_TO_P),
crosstrack_gain (XTRACK_GAIN_SCALED),
crosstrack_entry_angle (XTRACK_ENTRY_ANGLE_CENTIDEGREE),
altitude_mix (ALTITUDE_MIX),
airspeed_ratio (AIRSPEED_RATIO),
airspeed_offset (0),
/* XXX waypoint_mode missing here */
command_total (0),
command_index (0),
waypoint_radius (WP_RADIUS_DEFAULT),
loiter_radius (LOITER_RADIUS_DEFAULT),
#if GEOFENCE_ENABLED == ENABLED
fence_action (0),
fence_total (0),
fence_channel (0),
fence_minalt (0),
fence_maxalt (0),
#endif
flybywire_airspeed_min (AIRSPEED_FBW_MIN),
flybywire_airspeed_max (AIRSPEED_FBW_MAX),
throttle_min (THROTTLE_MIN),
throttle_max (THROTTLE_MAX),
throttle_slewrate (THROTTLE_SLEW_LIMIT),
throttle_fs_enabled (THROTTLE_FAILSAFE),
throttle_fs_value (THROTTLE_FS_VALUE),
throttle_cruise (THROTTLE_CRUISE),
short_fs_action (SHORT_FAILSAFE_ACTION),
long_fs_action (LONG_FAILSAFE_ACTION),
gcs_heartbeat_fs_enabled(GCS_HEARTBEAT_FAILSAFE),
flight_mode_channel (FLIGHT_MODE_CHANNEL),
flight_mode1 (FLIGHT_MODE_1),
flight_mode2 (FLIGHT_MODE_2),
flight_mode3 (FLIGHT_MODE_3),
flight_mode4 (FLIGHT_MODE_4),
flight_mode5 (FLIGHT_MODE_5),
flight_mode6 (FLIGHT_MODE_6),
roll_limit (HEAD_MAX_CENTIDEGREE),
pitch_limit_max (PITCH_MAX_CENTIDEGREE),
pitch_limit_min (PITCH_MIN_CENTIDEGREE),
auto_trim (AUTO_TRIM),
switch_enable (REVERSE_SWITCH),
mix_mode (ELEVON_MIXING),
reverse_elevons (ELEVON_REVERSE),
reverse_ch1_elevon (ELEVON_CH1_REVERSE),
reverse_ch2_elevon (ELEVON_CH2_REVERSE),
num_resets (0),
log_bitmask (DEFAULT_LOG_BITMASK),
log_last_filenumber (0),
reset_switch_chan (0),
airspeed_cruise (AIRSPEED_CRUISE_CM),
min_gndspeed (MIN_GNDSPEED_CM),
pitch_trim (0),
RTL_altitude (ALT_HOLD_HOME_CM),
FBWB_min_altitude (ALT_HOLD_FBW_CM),
ground_temperature (0),
ground_pressure (0),
compass_enabled (MAGNETOMETER),
flap_1_percent (FLAP_1_PERCENT),
flap_1_speed (FLAP_1_SPEED),
flap_2_percent (FLAP_2_PERCENT),
flap_2_speed (FLAP_2_SPEED),
battery_monitoring (DISABLED),
volt_div_ratio (VOLT_DIV_RATIO),
curr_amp_per_volt (CURR_AMP_PER_VOLT),
input_voltage (INPUT_VOLTAGE),
pack_capacity (HIGH_DISCHARGE),
inverted_flight_ch (0),
sonar_enabled (SONAR_ENABLED),
airspeed_enabled (AIRSPEED_SENSOR),
// PID controller initial P initial I initial D initial imax
//-----------------------------------------------------------------------------------
pidNavRoll (NAV_ROLL_P, NAV_ROLL_I, NAV_ROLL_D, NAV_ROLL_INT_MAX_CENTIDEGREE),
pidServoRoll (SERVO_ROLL_P, SERVO_ROLL_I, SERVO_ROLL_D, SERVO_ROLL_INT_MAX_CENTIDEGREE),
pidServoPitch (SERVO_PITCH_P, SERVO_PITCH_I, SERVO_PITCH_D, SERVO_PITCH_INT_MAX_CENTIDEGREE),
pidNavPitchAirspeed (NAV_PITCH_ASP_P, NAV_PITCH_ASP_I, NAV_PITCH_ASP_D, NAV_PITCH_ASP_INT_MAX_CMSEC),
pidServoRudder (SERVO_YAW_P, SERVO_YAW_I, SERVO_YAW_D, SERVO_YAW_INT_MAX),
pidTeThrottle (THROTTLE_TE_P, THROTTLE_TE_I, THROTTLE_TE_D, THROTTLE_TE_INT_MAX),
pidNavPitchAltitude (NAV_PITCH_ALT_P, NAV_PITCH_ALT_I, NAV_PITCH_ALT_D, NAV_PITCH_ALT_INT_MAX_CM)
{}
};
#endif // PARAMETERS_H

163
Tools/VARTest/Parameters.pde Normal file
View File

@ -0,0 +1,163 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
/*
ArduPlane parameter definitions
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.
*/
#define GSCALAR(v, name) { g.v.vtype, name, Parameters::k_param_ ## v, &g.v }
#define GGROUP(v, name, class) { AP_PARAM_GROUP, name, Parameters::k_param_ ## v, &g.v, class::var_info }
#define GOBJECT(v, name, class) { AP_PARAM_GROUP, name, Parameters::k_param_ ## v, &v, class::var_info }
static const AP_Param::Info var_info[] PROGMEM = {
GSCALAR(format_version, "FORMAT_VERSION"),
GSCALAR(software_type, "SYSID_SW_TYPE"),
GSCALAR(sysid_this_mav, "SYSID_THISMAV"),
GSCALAR(sysid_my_gcs, "SYSID_MYGCS"),
GSCALAR(serial3_baud, "SERIAL3_BAUD"),
GSCALAR(kff_pitch_compensation, "KFF_PTCHCOMP"),
GSCALAR(kff_rudder_mix, "KFF_RDDRMIX"),
GSCALAR(kff_pitch_to_throttle, "KFF_PTCH2THR"),
GSCALAR(kff_throttle_to_pitch, "KFF_THR2PTCH"),
GSCALAR(crosstrack_gain, "XTRK_GAIN_SC"),
GSCALAR(crosstrack_entry_angle, "XTRK_ANGLE_CD"),
GSCALAR(altitude_mix, "ALT_MIX"),
GSCALAR(airspeed_ratio, "ARSPD_RATIO"),
GSCALAR(airspeed_offset, "ARSPD_OFFSET"),
GSCALAR(command_total, "CMD_TOTAL"),
GSCALAR(command_index, "CMD_INDEX"),
GSCALAR(waypoint_radius, "WP_RADIUS"),
GSCALAR(loiter_radius, "WP_LOITER_RAD"),
#if GEOFENCE_ENABLED == ENABLED
GSCALAR(fence_action, "FENCE_ACTION"),
GSCALAR(fence_total, "FENCE_TOTAL"),
GSCALAR(fence_channel, "FENCE_CHANNEL"),
GSCALAR(fence_minalt, "FENCE_MINALT"),
GSCALAR(fence_maxalt, "FENCE_MAXALT"),
#endif
GSCALAR(flybywire_airspeed_min, "ARSPD_FBW_MIN"),
GSCALAR(flybywire_airspeed_max, "ARSPD_FBW_MAX"),
GSCALAR(throttle_min, "THR_MIN"),
GSCALAR(throttle_max, "THR_MAX"),
GSCALAR(throttle_slewrate, "THR_SLEWRATE"),
GSCALAR(throttle_fs_enabled, "THR_FAILSAFE"),
GSCALAR(throttle_fs_value, "THR_FS_VALUE"),
GSCALAR(throttle_cruise, "TRIM_THROTTLE"),
GSCALAR(short_fs_action, "FS_SHORT_ACTN"),
GSCALAR(long_fs_action, "FS_LONG_ACTN"),
GSCALAR(gcs_heartbeat_fs_enabled, "FS_GCS_ENABL"),
GSCALAR(flight_mode_channel, "FLTMODE_CH"),
GSCALAR(flight_mode1, "FLTMODE1"),
GSCALAR(flight_mode2, "FLTMODE2"),
GSCALAR(flight_mode3, "FLTMODE3"),
GSCALAR(flight_mode4, "FLTMODE4"),
GSCALAR(flight_mode5, "FLTMODE5"),
GSCALAR(flight_mode6, "FLTMODE6"),
GSCALAR(roll_limit, "LIM_ROLL_CD"),
GSCALAR(pitch_limit_max, "LIM_PITCH_MAX"),
GSCALAR(pitch_limit_min, "LIM_PITCH_MIN"),
GSCALAR(auto_trim, "TRIM_AUTO"),
GSCALAR(switch_enable, "SWITCH_ENABLE"),
GSCALAR(mix_mode, "ELEVON_MIXING"),
GSCALAR(reverse_elevons, "ELEVON_REVERSE"),
GSCALAR(reverse_ch1_elevon, "ELEVON_CH1_REV"),
GSCALAR(reverse_ch2_elevon, "ELEVON_CH2_REV"),
GSCALAR(num_resets, "SYS_NUM_RESETS"),
GSCALAR(log_bitmask, "LOG_BITMASK"),
GSCALAR(log_last_filenumber, "LOG_LASTFILE"),
GSCALAR(reset_switch_chan, "RST_SWITCH_CH"),
GSCALAR(airspeed_cruise, "TRIM_ARSPD_CM"),
GSCALAR(min_gndspeed, "MIN_GNDSPD_CM"),
GSCALAR(pitch_trim, "TRIM_PITCH_CD"),
GSCALAR(RTL_altitude, "ALT_HOLD_RTL"),
GSCALAR(FBWB_min_altitude, "ALT_HOLD_FBWCM"),
GSCALAR(ground_temperature, "GND_TEMP"),
GSCALAR(ground_pressure, "GND_ABS_PRESS"),
GSCALAR(compass_enabled, "MAG_ENABLE"),
GSCALAR(flap_1_percent, "FLAP_1_PERCNT"),
GSCALAR(flap_1_speed, "FLAP_1_SPEED"),
GSCALAR(flap_2_percent, "FLAP_2_PERCNT"),
GSCALAR(flap_2_speed, "FLAP_2_SPEED"),
GSCALAR(battery_monitoring, "BATT_MONITOR"),
GSCALAR(volt_div_ratio, "VOLT_DIVIDER"),
GSCALAR(curr_amp_per_volt, "AMP_PER_VOLT"),
GSCALAR(input_voltage, "INPUT_VOLTS"),
GSCALAR(pack_capacity, "BATT_CAPACITY"),
GSCALAR(inverted_flight_ch, "INVERTEDFLT_CH"),
GSCALAR(sonar_enabled, "SONAR_ENABLE"),
GSCALAR(airspeed_enabled, "ARSPD_ENABLE"),
GGROUP(channel_roll, "RC1_", RC_Channel),
GGROUP(channel_pitch, "RC2_", RC_Channel),
GGROUP(channel_throttle, "RC3_", RC_Channel),
GGROUP(channel_rudder, "RC4_", RC_Channel),
GGROUP(rc_5, "RC5_", RC_Channel_aux),
GGROUP(rc_6, "RC6_", RC_Channel_aux),
GGROUP(rc_7, "RC7_", RC_Channel_aux),
GGROUP(rc_8, "RC8_", RC_Channel_aux),
GGROUP(pidNavRoll, "HDNG2RLL_", PID),
GGROUP(pidServoRoll, "RLL2SRV_", PID),
GGROUP(pidServoPitch, "PTCH2SRV_", PID),
GGROUP(pidNavPitchAirspeed, "ARSP2PTCH_", PID),
GGROUP(pidServoRudder, "YW2SRV_", PID),
GGROUP(pidTeThrottle, "ENRGY2THR_", PID),
GGROUP(pidNavPitchAltitude, "ALT2PTCH_", PID),
// variables not in the g class which contain EEPROM saved variables
GOBJECT(compass, "COMPASS_", Compass),
#if 0
// VARTest doesn't have these
GOBJECT(gcs0, "SR0_", GCS_MAVLINK),
GOBJECT(gcs3, "SR3_", GCS_MAVLINK)
#endif
};
static void load_parameters(void)
{
// setup the AP_Var subsystem for storage to EEPROM
if (!AP_Param::setup(var_info, sizeof(var_info)/sizeof(var_info[0]), WP_START_BYTE)) {
// this can only happen on startup, and its a definate coding
// error. Best not to continue so the programmer catches it
while (1) {
Serial.println_P(PSTR("ERROR: Failed to setup AP_Param"));
delay(1000);
}
}
if (!g.format_version.load() ||
g.format_version != Parameters::k_format_version) {
// erase all parameters
Serial.printf_P(PSTR("Firmware change (%u -> %u): erasing EEPROM...\n"),
g.format_version.get(), Parameters::k_format_version);
AP_Param::erase_all();
// save the current format version
g.format_version.set_and_save(Parameters::k_format_version);
Serial.println_P(PSTR("done."));
} else {
unsigned long before = micros();
// Load all auto-loaded EEPROM variables
AP_Param::load_all();
Serial.printf_P(PSTR("load_all took %luus\n"), micros() - before);
}
}

314
Tools/VARTest/VARTest.pde Normal file
View File

@ -0,0 +1,314 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
/*
new variable scheme
Andrew Tridgell February 2012
*/
#include <FastSerial.h>
#include <AP_Common.h>
#include <Arduino_Mega_ISR_Registry.h>
#include <PID.h> // PID library
#include <APM_RC.h> // ArduPilot Mega RC Library
#include <RC_Channel.h> // RC Channel Library
#include <AP_Compass.h> // ArduPilot Mega Magnetometer Library
#include <SPI.h> // Arduino SPI lib
#include <I2C.h>
#include <DataFlash.h>
#include <AP_DCM.h>
#include <AP_ADC.h>
#include <AP_Baro.h>
#include <AP_PeriodicProcess.h>
#include <AP_InertialSensor.h> // Inertial Sensor (uncalibated IMU) Library
#include <AP_IMU.h> // ArduPilot Mega IMU Library
#include <AP_GPS.h>
#include <AP_Math.h>
#include <config.h>
#include <Parameters.h>
static Parameters g;
static AP_ADC_ADS7844 adc;
static GPS *g_gps;
AP_GPS_Auto g_gps_driver(&Serial1, &g_gps);
# if CONFIG_IMU_TYPE == CONFIG_IMU_MPU6000
AP_InertialSensor_MPU6000 ins( CONFIG_MPU6000_CHIP_SELECT_PIN );
# else
AP_InertialSensor_Oilpan ins( &adc );
#endif // CONFIG_IMU_TYPE
AP_IMU_INS imu( &ins );
AP_DCM dcm(&imu, g_gps);
Arduino_Mega_ISR_Registry isr_registry;
#ifdef DESKTOP_BUILD
AP_Compass_HIL compass;
#else
static AP_Compass_HMC5843 compass;
#endif
AP_Baro_BMP085_HIL barometer;
FastSerialPort0(Serial);
FastSerialPort1(Serial1); // GPS port
#define SERIAL0_BAUD 115200
#define Debug(fmt, args...) Serial.printf("%s:%d: " fmt "\n", __FUNCTION__, __LINE__ , ##args)
void setup() {
Serial.begin(SERIAL0_BAUD, 128, 128);
load_parameters();
// show some sizes
Serial.printf_P(PSTR("sizeof(RC_Channel)=%u\n"), (unsigned)sizeof(RC_Channel));
Serial.printf_P(PSTR("sizeof(g)=%u\n"), (unsigned)sizeof(g));
Serial.printf_P(PSTR("sizeof(g.throttle_min)=%u\n"), (unsigned)sizeof(g.throttle_min));
Serial.printf_P(PSTR("sizeof(g.channel_roll)=%u\n"), (unsigned)sizeof(g.channel_roll));
Serial.printf_P(PSTR("throttle_max now: %u\n"), (unsigned)g.throttle_max);
// some ad-hoc testing
// try set interfaces
g.throttle_min.set(g.throttle_min+1);
g.throttle_min.save();
g.throttle_min.set_and_save(g.throttle_min+1);
Serial.printf_P(PSTR("throttle_min now: %u\n"), (unsigned)g.throttle_min);
// find a variable by name
AP_Param *vp;
enum ap_var_type type;
vp = AP_Param::find("RLL2SRV_P", &type);
((AP_Float *)vp)->set(23);
Serial.printf_P(PSTR("RLL2SRV_P=%f\n"),
g.pidServoRoll.kP());
char s[AP_MAX_NAME_SIZE+1];
g.throttle_min.copy_name(s, sizeof(s));
s[AP_MAX_NAME_SIZE] = 0;
Serial.printf_P(PSTR("THROTTLE_MIN.copy_name()->%s\n"), s);
g.channel_roll.radio_min.copy_name(s, sizeof(s));
s[AP_MAX_NAME_SIZE] = 0;
Serial.printf_P(PSTR("RC1_MIN.copy_name()->%s %p\n"), s, &g.channel_roll.radio_min);
Vector3f ofs;
ofs = compass.get_offsets();
Serial.printf_P(PSTR("Compass: %f %f %f\n"),
ofs.x, ofs.y, ofs.z);
ofs.x += 1.1;
ofs.y += 1.2;
ofs.z += 1.3;
compass.set_offsets(ofs);
compass.save_offsets();
// full testing of all variables
uint16_t token;
for (AP_Param *ap = AP_Param::first(&token, &type);
ap;
ap=AP_Param::next(&token, &type)) {
test_variable(ap, type);
}
AP_Param::show_all();
Serial.println_P(PSTR("All done."));
}
void loop()
{
}
// test all interfaces for a variable
void test_variable(AP_Param *ap, enum ap_var_type type)
{
static uint8_t value;
char s[AP_MAX_NAME_SIZE+1];
value++;
ap->copy_name(s, sizeof(s));
Serial.printf_P(PSTR("Testing variable '%s' of type %u\n"),
s, type);
enum ap_var_type type2;
if (AP_Param::find(s, &type2) != ap ||
type2 != type) {
Debug("find failed");
}
if (strcmp(s, "FORMAT_VERSION") == 0) {
// don't wipe the version
return;
}
switch (type) {
case AP_PARAM_INT8: {
AP_Int8 *v = (AP_Int8 *)ap;
if (sizeof(*v) != 1) {
Debug("incorrect size %u", (unsigned)sizeof(*v));
}
v->set(value);
if (!v->save()) {
Debug("failed set_and_save");
}
if (!v->load()) {
Debug("failed load");
}
if (v->get() != value) {
Debug("wrong value %u %u", (unsigned)v->get(), value);
}
if (!v->set_and_save(value+1)) {
Debug("failed set_and_save");
}
if (!v->load()) {
Debug("failed load");
}
if (v->vtype != type) {
Debug("wrong type");
}
if (v->get() != value+1) {
Debug("wrong value %u %u", (unsigned)v->get(), value+1);
}
if (*v != value+1) {
Debug("wrong direct value %u %u", (unsigned)v->get(), value+1);
}
*v = value+2;
if (v->get() != value+2) {
Debug("wrong copy assignment value %u %u", (unsigned)v->get(), value+2);
}
break;
}
case AP_PARAM_INT16: {
AP_Int16 *v = (AP_Int16 *)ap;
if (sizeof(*v) != 2) {
Debug("incorrect size %u", (unsigned)sizeof(*v));
}
v->set(value);
if (!v->save()) {
Debug("failed set_and_save");
}
if (!v->load()) {
Debug("failed load");
}
if (v->get() != value) {
Debug("wrong value %u %u", (unsigned)v->get(), value);
}
if (!v->set_and_save(value+1)) {
Debug("failed set_and_save");
}
if (!v->load()) {
Debug("failed load");
}
if (v->vtype != type) {
Debug("wrong type");
}
if (v->get() != value+1) {
Debug("wrong value %u %u", (unsigned)v->get(), value+1);
}
if (*v != value+1) {
Debug("wrong direct value %u %u", (unsigned)v->get(), value+1);
}
*v = value+2;
if (v->get() != value+2) {
Debug("wrong copy assignment value %u %u", (unsigned)v->get(), value+2);
}
break;
}
case AP_PARAM_INT32: {
AP_Int32 *v = (AP_Int32 *)ap;
if (sizeof(*v) != 4) {
Debug("incorrect size %u", (unsigned)sizeof(*v));
}
v->set(value);
if (!v->save()) {
Debug("failed set_and_save");
}
if (!v->load()) {
Debug("failed load");
}
if (v->get() != value) {
Debug("wrong value %u %u", (unsigned)v->get(), value);
}
if (!v->set_and_save(value+1)) {
Debug("failed set_and_save");
}
if (!v->load()) {
Debug("failed load");
}
if (v->vtype != type) {
Debug("wrong type");
}
if (v->get() != value+1) {
Debug("wrong value %u %u", (unsigned)v->get(), value+1);
}
if (*v != value+1) {
Debug("wrong direct value %u %u", (unsigned)v->get(), value+1);
}
*v = value+2;
if (v->get() != value+2) {
Debug("wrong copy assignment value %u %u", (unsigned)v->get(), value+2);
}
break;
}
case AP_PARAM_FLOAT: {
AP_Float *v = (AP_Float *)ap;
if (sizeof(*v) != 4) {
Debug("incorrect size %u", (unsigned)sizeof(*v));
}
v->set(value);
if (!v->save()) {
Debug("failed set_and_save");
}
if (!v->load()) {
Debug("failed load");
}
if (v->get() != value) {
Debug("wrong value %u %u", (unsigned)v->get(), value);
}
if (!v->set_and_save(value+1)) {
Debug("failed set_and_save");
}
if (!v->load()) {
Debug("failed load");
}
if (v->vtype != type) {
Debug("wrong type");
}
if (v->get() != value+1) {
Debug("wrong value %u %u", (unsigned)v->get(), value+1);
}
if (*v != value+1) {
Debug("wrong direct value %u %u", (unsigned)v->get(), value+1);
}
*v = value+2;
if (v->get() != value+2) {
Debug("wrong copy assignment value %u %u", (unsigned)v->get(), value+2);
}
break;
}
case AP_PARAM_VECTOR3F: {
AP_Vector3f *v = (AP_Vector3f *)ap;
if (sizeof(*v) != 12) {
Debug("incorrect size %u", (unsigned)sizeof(*v));
}
break;
}
case AP_PARAM_VECTOR6F: {
AP_Vector6f *v = (AP_Vector6f *)ap;
if (sizeof(*v) != 24) {
Debug("incorrect size %u", (unsigned)sizeof(*v));
}
break;
}
case AP_PARAM_MATRIX3F: {
AP_Matrix3f *v = (AP_Matrix3f *)ap;
if (sizeof(*v) != 36) {
Debug("incorrect size %u", (unsigned)sizeof(*v));
}
break;
}
default:
break;
}
}

836
Tools/VARTest/config.h Normal file
View File

@ -0,0 +1,836 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
//
//////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////
//
// WARNING WARNING WARNING WARNING WARNING WARNING WARNING WARNING WARNING
//
// DO NOT EDIT this file to adjust your configuration. Create your own
// APM_Config.h and use APM_Config.h.example as a reference.
//
// WARNING WARNING WARNING WARNING WARNING WARNING WARNING WARNING WARNING
///
//////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////
//
// Default and automatic configuration details.
//
// Notes for maintainers:
//
// - Try to keep this file organised in the same order as APM_Config.h.example
//
#include "defines.h"
///
/// 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. EVER.
///
/// 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
#define DISABLED 0
//////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////
// HARDWARE CONFIGURATION AND CONNECTIONS
//////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////
// APM HARDWARE
//
#ifndef CONFIG_APM_HARDWARE
# define CONFIG_APM_HARDWARE APM_HARDWARE_APM1
#endif
#if defined( __AVR_ATmega1280__ )
#define CLI_ENABLED DISABLED
#define LOGGING_ENABLED DISABLED
#endif
//////////////////////////////////////////////////////////////////////////////
// APM2 HARDWARE DEFAULTS
//
#if CONFIG_APM_HARDWARE == APM_HARDWARE_APM2
# define CONFIG_IMU_TYPE CONFIG_IMU_MPU6000
# define CONFIG_PUSHBUTTON DISABLED
# define CONFIG_RELAY DISABLED
# define MAG_ORIENTATION AP_COMPASS_APM2_SHIELD
# define CONFIG_SONAR_SOURCE SONAR_SOURCE_ANALOG_PIN
# define CONFIG_PITOT_SOURCE PITOT_SOURCE_ANALOG_PIN
# define MAGNETOMETER ENABLED
# ifdef APM2_BETA_HARDWARE
# define CONFIG_BARO AP_BARO_BMP085
# else // APM2 Production Hardware (default)
# define CONFIG_BARO AP_BARO_MS5611
# endif
#endif
//////////////////////////////////////////////////////////////////////////////
// LED and IO Pins
//
#if CONFIG_APM_HARDWARE == APM_HARDWARE_APM1
# define A_LED_PIN 37
# define B_LED_PIN 36
# define C_LED_PIN 35
# define LED_ON HIGH
# define LED_OFF LOW
# define SLIDE_SWITCH_PIN 40
# define PUSHBUTTON_PIN 41
# define USB_MUX_PIN -1
# define CONFIG_RELAY ENABLED
# define BATTERY_PIN_1 0
# define CURRENT_PIN_1 1
#elif CONFIG_APM_HARDWARE == APM_HARDWARE_APM2
# define A_LED_PIN 27
# define B_LED_PIN 26
# define C_LED_PIN 25
# define LED_ON LOW
# define LED_OFF HIGH
# define SLIDE_SWITCH_PIN (-1)
# define PUSHBUTTON_PIN (-1)
# define CLI_SLIDER_ENABLED DISABLED
# define USB_MUX_PIN 23
# define BATTERY_PIN_1 1
# 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
//
#ifndef CONFIG_IMU_TYPE
# define CONFIG_IMU_TYPE CONFIG_IMU_OILPAN
#endif
#if CONFIG_IMU_TYPE == CONFIG_IMU_MPU6000
# ifndef CONFIG_MPU6000_CHIP_SELECT_PIN
# define CONFIG_MPU6000_CHIP_SELECT_PIN 53
# endif
#endif
//////////////////////////////////////////////////////////////////////////////
// ADC Enable - used to eliminate for systems which don't have ADC.
//
#ifndef CONFIG_ADC
# if CONFIG_IMU_TYPE == CONFIG_IMU_OILPAN
# define CONFIG_ADC ENABLED
# else
# define CONFIG_ADC DISABLED
# 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
#ifndef SONAR_ENABLED
#define SONAR_ENABLED DISABLED
#endif
//////////////////////////////////////////////////////////////////////////////
// HIL_MODE OPTIONAL
#ifndef HIL_MODE
#define HIL_MODE HIL_MODE_DISABLED
#endif
#if HIL_MODE != HIL_MODE_DISABLED // we are in HIL mode
# undef GPS_PROTOCOL
# define GPS_PROTOCOL GPS_PROTOCOL_NONE
#endif
//////////////////////////////////////////////////////////////////////////////
// GPS_PROTOCOL
//
// Note that this test must follow the HIL_PROTOCOL block as the HIL
// setup may override the GPS configuration.
//
#ifndef GPS_PROTOCOL
# define GPS_PROTOCOL GPS_PROTOCOL_AUTO
#endif
#ifndef MAV_SYSTEM_ID
# define MAV_SYSTEM_ID 1
#endif
//////////////////////////////////////////////////////////////////////////////
// Serial port speeds.
//
#ifndef SERIAL0_BAUD
# define SERIAL0_BAUD 115200
#endif
#ifndef SERIAL3_BAUD
# define SERIAL3_BAUD 57600
#endif
//////////////////////////////////////////////////////////////////////////////
// Battery monitoring
//
#ifndef BATTERY_EVENT
# define BATTERY_EVENT DISABLED
#endif
#ifndef LOW_VOLTAGE
# define LOW_VOLTAGE 9.6
#endif
#ifndef VOLT_DIV_RATIO
# define VOLT_DIV_RATIO 3.56 // This is the proper value for an on-board APM1 voltage divider with a 3.9kOhm resistor
//# define VOLT_DIV_RATIO 15.70 // This is the proper value for the AttoPilot 50V/90A sensor
//# define VOLT_DIV_RATIO 4.127 // This is the proper value for the AttoPilot 13.6V/45A sensor
#endif
#ifndef CURR_AMP_PER_VOLT
# define CURR_AMP_PER_VOLT 27.32 // This is the proper value for the AttoPilot 50V/90A sensor
//# define CURR_AMP_PER_VOLT 13.66 // This is the proper value for the AttoPilot 13.6V/45A sensor
#endif
#ifndef CURR_AMPS_OFFSET
# define CURR_AMPS_OFFSET 0.0
#endif
#ifndef HIGH_DISCHARGE
# define HIGH_DISCHARGE 1760
#endif
//////////////////////////////////////////////////////////////////////////////
// INPUT_VOLTAGE
//
#ifndef INPUT_VOLTAGE
# define INPUT_VOLTAGE 4.68 // 4.68 is the average value for a sample set. This is the value at the processor with 5.02 applied at the servo rail
#endif
//////////////////////////////////////////////////////////////////////////////
// MAGNETOMETER
#ifndef MAGNETOMETER
# define MAGNETOMETER DISABLED
#endif
#ifndef MAG_ORIENTATION
# define MAG_ORIENTATION AP_COMPASS_COMPONENTS_DOWN_PINS_FORWARD
#endif
//////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////
// RADIO CONFIGURATION
//////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////
// Radio channel limits
//
// Note that these are not called out in APM_Config.h.reference.
//
#ifndef CH5_MIN
# define CH5_MIN 1000
#endif
#ifndef CH5_MAX
# define CH5_MAX 2000
#endif
#ifndef CH6_MIN
# define CH6_MIN 1000
#endif
#ifndef CH6_MAX
# define CH6_MAX 2000
#endif
#ifndef CH7_MIN
# define CH7_MIN 1000
#endif
#ifndef CH7_MAX
# define CH7_MAX 2000
#endif
#ifndef CH8_MIN
# define CH8_MIN 1000
#endif
#ifndef CH8_MAX
# define CH8_MAX 2000
#endif
#ifndef FLAP_1_PERCENT
# define FLAP_1_PERCENT 0
#endif
#ifndef FLAP_1_SPEED
# define FLAP_1_SPEED 255
#endif
#ifndef FLAP_2_PERCENT
# define FLAP_2_PERCENT 0
#endif
#ifndef FLAP_2_SPEED
# define FLAP_2_SPEED 255
#endif
//////////////////////////////////////////////////////////////////////////////
// FLIGHT_MODE
// FLIGHT_MODE_CHANNEL
//
#ifndef FLIGHT_MODE_CHANNEL
# define FLIGHT_MODE_CHANNEL 8
#endif
#if (FLIGHT_MODE_CHANNEL != 5) && (FLIGHT_MODE_CHANNEL != 6) && (FLIGHT_MODE_CHANNEL != 7) && (FLIGHT_MODE_CHANNEL != 8)
# error XXX
# error XXX You must set FLIGHT_MODE_CHANNEL to 5, 6, 7 or 8
# error XXX
#endif
#if !defined(FLIGHT_MODE_1)
# define FLIGHT_MODE_1 RTL
#endif
#if !defined(FLIGHT_MODE_2)
# define FLIGHT_MODE_2 RTL
#endif
#if !defined(FLIGHT_MODE_3)
# define FLIGHT_MODE_3 STABILIZE
#endif
#if !defined(FLIGHT_MODE_4)
# define FLIGHT_MODE_4 STABILIZE
#endif
#if !defined(FLIGHT_MODE_5)
# define FLIGHT_MODE_5 MANUAL
#endif
#if !defined(FLIGHT_MODE_6)
# define FLIGHT_MODE_6 MANUAL
#endif
//////////////////////////////////////////////////////////////////////////////
// THROTTLE_FAILSAFE
// THROTTLE_FS_VALUE
// SHORT_FAILSAFE_ACTION
// LONG_FAILSAFE_ACTION
// GCS_HEARTBEAT_FAILSAFE
//
#ifndef THROTTLE_FAILSAFE
# define THROTTLE_FAILSAFE ENABLED
#endif
#ifndef THROTTLE_FS_VALUE
# define THROTTLE_FS_VALUE 950
#endif
#ifndef SHORT_FAILSAFE_ACTION
# define SHORT_FAILSAFE_ACTION 0
#endif
#ifndef LONG_FAILSAFE_ACTION
# define LONG_FAILSAFE_ACTION 0
#endif
#ifndef GCS_HEARTBEAT_FAILSAFE
# define GCS_HEARTBEAT_FAILSAFE DISABLED
#endif
//////////////////////////////////////////////////////////////////////////////
// AUTO_TRIM
//
#ifndef AUTO_TRIM
# define AUTO_TRIM DISABLED
#endif
//////////////////////////////////////////////////////////////////////////////
// THROTTLE_REVERSE
//
#ifndef THROTTLE_REVERSE
# define THROTTLE_REVERSE DISABLED
#endif
//////////////////////////////////////////////////////////////////////////////
// ENABLE_STICK_MIXING
//
#ifndef ENABLE_STICK_MIXING
# define ENABLE_STICK_MIXING ENABLED
#endif
//////////////////////////////////////////////////////////////////////////////
// THROTTLE_OUT
//
#ifndef THROTTE_OUT
# define THROTTLE_OUT ENABLED
#endif
//////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////
// STARTUP BEHAVIOUR
//////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////
// GROUND_START_DELAY
//
#ifndef GROUND_START_DELAY
# define GROUND_START_DELAY 0
#endif
//////////////////////////////////////////////////////////////////////////////
// ENABLE_AIR_START
//
#ifndef ENABLE_AIR_START
# define ENABLE_AIR_START DISABLED
#endif
//////////////////////////////////////////////////////////////////////////////
// ENABLE REVERSE_SWITCH
//
#ifndef REVERSE_SWITCH
# define REVERSE_SWITCH ENABLED
#endif
//////////////////////////////////////////////////////////////////////////////
// ENABLE ELEVON_MIXING
//
#ifndef ELEVON_MIXING
# define ELEVON_MIXING DISABLED
#endif
#ifndef ELEVON_REVERSE
# define ELEVON_REVERSE DISABLED
#endif
#ifndef ELEVON_CH1_REVERSE
# define ELEVON_CH1_REVERSE DISABLED
#endif
#ifndef ELEVON_CH2_REVERSE
# define ELEVON_CH2_REVERSE DISABLED
#endif
//////////////////////////////////////////////////////////////////////////////
// MOUNT (ANTENNA OR CAMERA)
//
#ifndef MOUNT
# define MOUNT DISABLED
#endif
#if defined( __AVR_ATmega1280__ ) && CAMERA == ENABLED
// The small ATmega1280 chip does not have enough memory for camera support
// so disable CLI, this will allow camera support and other improvements to fit.
// This should almost have no side effects, because the APM planner can now do a complete board setup.
#define CLI_ENABLED DISABLED
#endif
//////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////
// FLIGHT AND NAVIGATION CONTROL
//////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////
// Altitude measurement and control.
//
#ifndef ALT_EST_GAIN
# define ALT_EST_GAIN 0.01
#endif
#ifndef ALTITUDE_MIX
# define ALTITUDE_MIX 1
#endif
//////////////////////////////////////////////////////////////////////////////
// AIRSPEED_CRUISE
//
#ifndef AIRSPEED_CRUISE
# define AIRSPEED_CRUISE 12 // 12 m/s
#endif
#define AIRSPEED_CRUISE_CM AIRSPEED_CRUISE*100
//////////////////////////////////////////////////////////////////////////////
// MIN_GNDSPEED
//
#ifndef MIN_GNDSPEED
# define MIN_GNDSPEED 0 // m/s (0 disables)
#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
#endif
#ifndef THROTTLE_ALT_I
# define THROTTLE_ALT_I 0.0
#endif
#ifndef THROTTLE_ALT_D
# define THROTTLE_ALT_D 0.0
#endif
#ifndef THROTTLE_ALT_INT_MAX
# define THROTTLE_ALT_INT_MAX 20
#endif
#define THROTTLE_ALT_INT_MAX_CM THROTTLE_ALT_INT_MAX*100
*/
//////////////////////////////////////////////////////////////////////////////
// Servo Mapping
//
#ifndef THROTTLE_MIN
# define THROTTLE_MIN 0 // percent
#endif
#ifndef THROTTLE_CRUISE
# define THROTTLE_CRUISE 45
#endif
#ifndef THROTTLE_MAX
# define THROTTLE_MAX 75
#endif
//////////////////////////////////////////////////////////////////////////////
// Autopilot control limits
//
#ifndef HEAD_MAX
# define HEAD_MAX 45
#endif
#ifndef PITCH_MAX
# define PITCH_MAX 15
#endif
#ifndef PITCH_MIN
# define PITCH_MIN -25
#endif
#define HEAD_MAX_CENTIDEGREE HEAD_MAX * 100
#define PITCH_MAX_CENTIDEGREE PITCH_MAX * 100
#define PITCH_MIN_CENTIDEGREE PITCH_MIN * 100
//////////////////////////////////////////////////////////////////////////////
// Attitude control gains
//
#ifndef SERVO_ROLL_P
# define SERVO_ROLL_P 0.4
#endif
#ifndef SERVO_ROLL_I
# define SERVO_ROLL_I 0.0
#endif
#ifndef SERVO_ROLL_D
# define SERVO_ROLL_D 0.0
#endif
#ifndef SERVO_ROLL_INT_MAX
# define SERVO_ROLL_INT_MAX 5
#endif
#define SERVO_ROLL_INT_MAX_CENTIDEGREE SERVO_ROLL_INT_MAX*100
#ifndef ROLL_SLEW_LIMIT
# define ROLL_SLEW_LIMIT 0
#endif
#ifndef SERVO_PITCH_P
# define SERVO_PITCH_P 0.6
#endif
#ifndef SERVO_PITCH_I
# define SERVO_PITCH_I 0.0
#endif
#ifndef SERVO_PITCH_D
# define SERVO_PITCH_D 0.0
#endif
#ifndef SERVO_PITCH_INT_MAX
# define SERVO_PITCH_INT_MAX 5
#endif
#define SERVO_PITCH_INT_MAX_CENTIDEGREE SERVO_PITCH_INT_MAX*100
#ifndef PITCH_COMP
# define PITCH_COMP 0.2
#endif
#ifndef SERVO_YAW_P
# define SERVO_YAW_P 0.0
#endif
#ifndef SERVO_YAW_I
# define SERVO_YAW_I 0.0
#endif
#ifndef SERVO_YAW_D
# define SERVO_YAW_D 0.0
#endif
#ifndef SERVO_YAW_INT_MAX
# define SERVO_YAW_INT_MAX 0
#endif
#ifndef RUDDER_MIX
# define RUDDER_MIX 0.5
#endif
//////////////////////////////////////////////////////////////////////////////
// Navigation control gains
//
#ifndef NAV_ROLL_P
# define NAV_ROLL_P 0.7
#endif
#ifndef NAV_ROLL_I
# define NAV_ROLL_I 0.0
#endif
#ifndef NAV_ROLL_D
# define NAV_ROLL_D 0.02
#endif
#ifndef NAV_ROLL_INT_MAX
# define NAV_ROLL_INT_MAX 5
#endif
#define NAV_ROLL_INT_MAX_CENTIDEGREE NAV_ROLL_INT_MAX*100
#ifndef NAV_PITCH_ASP_P
# define NAV_PITCH_ASP_P 0.65
#endif
#ifndef NAV_PITCH_ASP_I
# define NAV_PITCH_ASP_I 0.0
#endif
#ifndef NAV_PITCH_ASP_D
# define NAV_PITCH_ASP_D 0.0
#endif
#ifndef NAV_PITCH_ASP_INT_MAX
# define NAV_PITCH_ASP_INT_MAX 5
#endif
#define NAV_PITCH_ASP_INT_MAX_CMSEC NAV_PITCH_ASP_INT_MAX*100
#ifndef NAV_PITCH_ALT_P
# define NAV_PITCH_ALT_P 0.65
#endif
#ifndef NAV_PITCH_ALT_I
# define NAV_PITCH_ALT_I 0.0
#endif
#ifndef NAV_PITCH_ALT_D
# define NAV_PITCH_ALT_D 0.0
#endif
#ifndef NAV_PITCH_ALT_INT_MAX
# define NAV_PITCH_ALT_INT_MAX 5
#endif
#define NAV_PITCH_ALT_INT_MAX_CM NAV_PITCH_ALT_INT_MAX*100
//////////////////////////////////////////////////////////////////////////////
// Energy/Altitude control gains
//
#ifndef THROTTLE_TE_P
# define THROTTLE_TE_P 0.50
#endif
#ifndef THROTTLE_TE_I
# define THROTTLE_TE_I 0.0
#endif
#ifndef THROTTLE_TE_D
# define THROTTLE_TE_D 0.0
#endif
#ifndef THROTTLE_TE_INT_MAX
# define THROTTLE_TE_INT_MAX 20
#endif
#ifndef THROTTLE_SLEW_LIMIT
# define THROTTLE_SLEW_LIMIT 0
#endif
#ifndef P_TO_T
# define P_TO_T 0
#endif
#ifndef T_TO_P
# define T_TO_P 0
#endif
#ifndef PITCH_TARGET
# define PITCH_TARGET 0
#endif
//////////////////////////////////////////////////////////////////////////////
// Crosstrack compensation
//
#ifndef XTRACK_GAIN
# define XTRACK_GAIN 1 // deg/m
#endif
#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
//////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////
// DEBUGGING
//////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////
// Dataflash logging control
//
#ifndef LOGGING_ENABLED
# define LOGGING_ENABLED ENABLED
#endif
#ifndef LOG_ATTITUDE_FAST
# define LOG_ATTITUDE_FAST DISABLED
#endif
#ifndef LOG_ATTITUDE_MED
# define LOG_ATTITUDE_MED ENABLED
#endif
#ifndef LOG_GPS
# define LOG_GPS ENABLED
#endif
#ifndef LOG_PM
# define LOG_PM ENABLED
#endif
#ifndef LOG_CTUN
# define LOG_CTUN DISABLED
#endif
#ifndef LOG_NTUN
# define LOG_NTUN DISABLED
#endif
#ifndef LOG_MODE
# define LOG_MODE ENABLED
#endif
#ifndef LOG_RAW
# define LOG_RAW DISABLED
#endif
#ifndef LOG_CMD
# define LOG_CMD ENABLED
#endif
#ifndef LOG_CUR
# define LOG_CUR DISABLED
#endif
// calculate the default log_bitmask
#define LOGBIT(_s) (LOG_##_s ? MASK_LOG_##_s : 0)
#define DEFAULT_LOG_BITMASK \
LOGBIT(ATTITUDE_FAST) | \
LOGBIT(ATTITUDE_MED) | \
LOGBIT(GPS) | \
LOGBIT(PM) | \
LOGBIT(CTUN) | \
LOGBIT(NTUN) | \
LOGBIT(MODE) | \
LOGBIT(RAW) | \
LOGBIT(CMD) | \
LOGBIT(CUR)
//////////////////////////////////////////////////////////////////////////////
// Navigation defaults
//
#ifndef WP_RADIUS_DEFAULT
# define WP_RADIUS_DEFAULT 30
#endif
#ifndef LOITER_RADIUS_DEFAULT
# define LOITER_RADIUS_DEFAULT 60
#endif
#ifndef ALT_HOLD_HOME
# define ALT_HOLD_HOME 100
#endif
#define ALT_HOLD_HOME_CM ALT_HOLD_HOME*100
#ifndef USE_CURRENT_ALT
# define USE_CURRENT_ALT FALSE
#endif
#ifndef INVERTED_FLIGHT_PWM
# define INVERTED_FLIGHT_PWM 1750
#endif
//////////////////////////////////////////////////////////////////////////////
// Developer Items
//
#ifndef STANDARD_SPEED
# define STANDARD_SPEED 15.0
#define STANDARD_SPEED_SQUARED (STANDARD_SPEED * STANDARD_SPEED)
#endif
#define STANDARD_THROTTLE_SQUARED (THROTTLE_CRUISE * THROTTLE_CRUISE)
// use this to enable servos in HIL mode
#ifndef HIL_SERVOS
# define HIL_SERVOS DISABLED
#endif
// use this to completely disable the CLI
#ifndef CLI_ENABLED
# define CLI_ENABLED ENABLED
#endif
// use this to disable the CLI slider switch
#ifndef CLI_SLIDER_ENABLED
# define CLI_SLIDER_ENABLED ENABLED
#endif
// delay to prevent Xbee bricking, in milliseconds
#ifndef MAVLINK_TELEMETRY_PORT_DELAY
# define MAVLINK_TELEMETRY_PORT_DELAY 2000
#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
// fence breach)
#ifndef RESET_SWITCH_CHAN_PWM
# define RESET_SWITCH_CHAN_PWM 1750
#endif

231
Tools/VARTest/defines.h Normal file
View File

@ -0,0 +1,231 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#ifndef _DEFINES_H
#define _DEFINES_H
// Internal defines, don't edit and expect things to work
// -------------------------------------------------------
#define TRUE 1
#define FALSE 0
#define ToRad(x) (x*0.01745329252) // *pi/180
#define ToDeg(x) (x*57.2957795131) // *180/pi
#define DEBUG 0
#define LOITER_RANGE 60 // for calculating power outside of loiter radius
#define SERVO_MAX 4500 // This value represents 45 degrees and is just an arbitrary representation of servo max travel.
// failsafe
// ----------------------
#define FAILSAFE_NONE 0
#define FAILSAFE_SHORT 1
#define FAILSAFE_LONG 2
#define FAILSAFE_GCS 3
#define FAILSAFE_SHORT_TIME 1500 // Miliiseconds
#define FAILSAFE_LONG_TIME 20000 // Miliiseconds
// active altitude sensor
// ----------------------
#define SONAR 0
#define BARO 1
#define PITOT_SOURCE_ADC 1
#define PITOT_SOURCE_ANALOG_PIN 2
#define T6 1000000
#define T7 10000000
// GPS type codes - use the names, not the numbers
#define GPS_PROTOCOL_NONE -1
#define GPS_PROTOCOL_NMEA 0
#define GPS_PROTOCOL_SIRF 1
#define GPS_PROTOCOL_UBLOX 2
#define GPS_PROTOCOL_IMU 3
#define GPS_PROTOCOL_MTK 4
#define GPS_PROTOCOL_HIL 5
#define GPS_PROTOCOL_MTK16 6
#define GPS_PROTOCOL_AUTO 7
#define CH_ROLL CH_1
#define CH_PITCH CH_2
#define CH_THROTTLE CH_3
#define CH_RUDDER CH_4
#define CH_YAW CH_4
// HIL enumerations
#define HIL_MODE_DISABLED 0
#define HIL_MODE_ATTITUDE 1
#define HIL_MODE_SENSORS 2
// Auto Pilot modes
// ----------------
#define MANUAL 0
#define CIRCLE 1 // When flying sans GPS, and we loose the radio, just circle
#define STABILIZE 2
#define FLY_BY_WIRE_A 5 // Fly By Wire A has left stick horizontal => desired roll angle, left stick vertical => desired pitch angle, right stick vertical = manual throttle
#define FLY_BY_WIRE_B 6 // Fly By Wire B has left stick horizontal => desired roll angle, left stick vertical => desired pitch angle, right stick vertical => desired airspeed
#define FLY_BY_WIRE_C 7 // Fly By Wire C has left stick horizontal => desired roll angle, left stick vertical => desired climb rate, right stick vertical => desired airspeed
// Fly By Wire B and Fly By Wire C require airspeed sensor
#define AUTO 10
#define RTL 11
#define LOITER 12
//#define TAKEOFF 13 // This is not used by APM. It appears here for consistency with ACM
//#define LAND 14 // This is not used by APM. It appears here for consistency with ACM
#define GUIDED 15
#define INITIALISING 16 // in startup routines
// Commands - Note that APM now uses a subset of the MAVLink protocol commands. See enum MAV_CMD in the GCS_Mavlink library
#define CMD_BLANK 0 // there is no command stored in the mem location requested
#define NO_COMMAND 0
#define WAIT_COMMAND 255
// Command/Waypoint/Location Options Bitmask
//--------------------
#define MASK_OPTIONS_RELATIVE_ALT (1<<0) // 1 = Relative altitude
//repeating events
#define NO_REPEAT 0
#define CH_5_TOGGLE 1
#define CH_6_TOGGLE 2
#define CH_7_TOGGLE 3
#define CH_8_TOGGLE 4
#define RELAY_TOGGLE 5
#define STOP_REPEAT 10
#define MAV_CMD_CONDITION_YAW 23
// GCS Message ID's
/// NOTE: to ensure we never block on sending MAVLink messages
/// please keep each MSG_ to a single MAVLink message. If need be
/// create new MSG_ IDs for additional messages on the same
/// stream
enum ap_message {
MSG_HEARTBEAT,
MSG_ATTITUDE,
MSG_LOCATION,
MSG_EXTENDED_STATUS1,
MSG_EXTENDED_STATUS2,
MSG_NAV_CONTROLLER_OUTPUT,
MSG_CURRENT_WAYPOINT,
MSG_VFR_HUD,
MSG_RADIO_OUT,
MSG_RADIO_IN,
MSG_RAW_IMU1,
MSG_RAW_IMU2,
MSG_RAW_IMU3,
MSG_GPS_STATUS,
MSG_GPS_RAW,
MSG_SERVO_OUT,
MSG_NEXT_WAYPOINT,
MSG_NEXT_PARAM,
MSG_STATUSTEXT,
MSG_FENCE_STATUS,
MSG_RETRY_DEFERRED // this must be last
};
enum gcs_severity {
SEVERITY_LOW=1,
SEVERITY_MEDIUM,
SEVERITY_HIGH,
SEVERITY_CRITICAL
};
// Logging parameters
#define LOG_INDEX_MSG 0xF0
#define LOG_ATTITUDE_MSG 0x01
#define LOG_GPS_MSG 0x02
#define LOG_MODE_MSG 0X03
#define LOG_CONTROL_TUNING_MSG 0X04
#define LOG_NAV_TUNING_MSG 0X05
#define LOG_PERFORMANCE_MSG 0X06
#define LOG_RAW_MSG 0x07
#define LOG_CMD_MSG 0x08
#define LOG_CURRENT_MSG 0x09
#define LOG_STARTUP_MSG 0x0A
#define TYPE_AIRSTART_MSG 0x00
#define TYPE_GROUNDSTART_MSG 0x01
#define MAX_NUM_LOGS 100
#define MASK_LOG_ATTITUDE_FAST (1<<0)
#define MASK_LOG_ATTITUDE_MED (1<<1)
#define MASK_LOG_GPS (1<<2)
#define MASK_LOG_PM (1<<3)
#define MASK_LOG_CTUN (1<<4)
#define MASK_LOG_NTUN (1<<5)
#define MASK_LOG_MODE (1<<6)
#define MASK_LOG_RAW (1<<7)
#define MASK_LOG_CMD (1<<8)
#define MASK_LOG_CUR (1<<9)
// Waypoint Modes
// ----------------
#define ABS_WP 0
#define REL_WP 1
// Command Queues
// ---------------
#define COMMAND_MUST 0
#define COMMAND_MAY 1
#define COMMAND_NOW 2
// Events
// ------
#define EVENT_WILL_REACH_WAYPOINT 1
#define EVENT_SET_NEW_COMMAND_INDEX 2
#define EVENT_LOADED_WAYPOINT 3
#define EVENT_LOOP 4
// Climb rate calculations
#define ALTITUDE_HISTORY_LENGTH 8 //Number of (time,altitude) points to regress a climb rate from
#define BATTERY_VOLTAGE(x) (x*(g.input_voltage/1024.0))*g.volt_div_ratio
#define CURRENT_AMPS(x) ((x*(g.input_voltage/1024.0))-CURR_AMPS_OFFSET)*g.curr_amp_per_volt
#define RELAY_PIN 47
// sonar
#define MAX_SONAR_XL 0
#define MAX_SONAR_LV 1
#define SonarToCm(x) (x*1.26) // Sonar raw value to centimeters
#define AN4 4
#define AN5 5
#define SPEEDFILT 400 // centimeters/second; the speed below which a groundstart will be triggered
// 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_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 ONBOARD_PARAM_NAME_LENGTH 15
// convert a boolean (0 or 1) to a sign for multiplying (0 maps to 1, 1 maps to -1)
#define BOOL_TO_SIGN(bvalue) ((bvalue)?-1:1)
// mark a function as not to be inlined
#define NOINLINE __attribute__((noinline))
#define CONFIG_IMU_OILPAN 1
#define CONFIG_IMU_MPU6000 2
#define APM_HARDWARE_APM1 1
#define APM_HARDWARE_APM2 2
#define AP_BARO_BMP085 1
#define AP_BARO_MS5611 2
#endif // _DEFINES_H

14
Tools/autotest/ArduPlane.parm
View File

@ -1,17 +1,17 @@
LOG_BITMASK 4095
SWITCH_ENABLE 0
MAG_ENABLE 0
MAG_ENABLE 1
TRIM_ARSPD_CM 2200
TRIM_PITCH_CD -1000
ARSPD_ENABLE 1
ARSP2PTCH_I 0.1
ARSPD_FBW_MAX 30
ARSPD_FBW_MIN 10
KFF_RDDRMIX 0
KFF_PTCHCOMP 0.2
KFF_RDDRMIX 0.5
KFF_PTCHCOMP 0.3
THR_MAX 100
HDNG2RLL_D 0.5
HDNG2RLL_I 0.0
HDNG2RLL_I 0.01
HDNG2RLL_IMAX 50.0
HDNG2RLL_P 1.50
RC2_REV -1
@ -48,8 +48,10 @@ FLTMODE5 2
FLTMODE6 0
FLTMODE_CH 8
PTCH2SRV_P 1.5
RLL2SRV_I 0
RLL2SRV_I 0.01
RLL2SRV_IMAX 100
RLL2SRV_P 0.8
XTRK_ANGLE_CD 500
XTRK_GAIN_SC 25
WP_LOITER_RAD 90
YW2SRV_P 1.0

2
Tools/autotest/ap1.txt
View File

@ -3,7 +3,7 @@ QGC WPL 110
1 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.361553 149.163956 100.000000 1
2 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.364540 149.162857 100.000000 1
3 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.361721 149.161835 40.000000 1
4 0 3 178 0.000000 12.00000 0.000000 0.000000 0.000000 0.000000 12.000000 1
4 0 3 178 0.000000 12.00000 0.000000 0.000000 0.000000 0.000000 15.000000 1
5 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.367970 149.164124 25.000000 1
6 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.366814 149.165878 20.000000 1
7 0 3 21 0.000000 0.000000 0.000000 0.000000 -35.362911 149.165222 0.000000 1

63
Tools/autotest/arduplane.py
View File

@ -42,7 +42,8 @@ def takeoff(mavproxy, mav):
mavproxy.send('rc 3 1800\n')
# gain a bit of altitude
wait_altitude(mav, homeloc.alt+30, homeloc.alt+60, timeout=30)
if not wait_altitude(mav, homeloc.alt+30, homeloc.alt+60, timeout=30):
return False
# level off
mavproxy.send('rc 2 1500\n')
@ -55,7 +56,8 @@ def fly_left_circuit(mavproxy, mav):
mavproxy.send('switch 4\n')
wait_mode(mav, 'FBWA')
mavproxy.send('rc 3 2000\n')
wait_level_flight(mavproxy, mav)
if not wait_level_flight(mavproxy, mav):
return False
print("Flying left circuit")
# do 4 turns
@ -63,10 +65,12 @@ def fly_left_circuit(mavproxy, mav):
# hard left
print("Starting turn %u" % i)
mavproxy.send('rc 1 1000\n')
wait_heading(mav, 270 - (90*i))
if not wait_heading(mav, 270 - (90*i)):
return False
mavproxy.send('rc 1 1500\n')
print("Starting leg %u" % i)
wait_distance(mav, 100)
if not wait_distance(mav, 100):
return False
print("Circuit complete")
return True
@ -75,31 +79,40 @@ def fly_RTL(mavproxy, mav):
print("Flying home in RTL")
mavproxy.send('switch 2\n')
wait_mode(mav, 'RTL')
wait_location(mav, homeloc, accuracy=90,
target_altitude=100, height_accuracy=20)
if not wait_location(mav, homeloc, accuracy=90,
target_altitude=homeloc.alt+100, height_accuracy=20,
timeout=90):
return False
print("RTL Complete")
return True
def fly_LOITER(mavproxy, mav):
def fly_LOITER(mavproxy, mav, num_circles=4):
'''loiter where we are'''
print("Testing LOITER")
print("Testing LOITER for %u turns" % num_circles)
mavproxy.send('switch 3\n')
wait_mode(mav, 'LOITER')
while True:
wait_heading(mav, 0)
wait_heading(mav, 180)
while num_circles > 0:
if not wait_heading(mav, 0):
return False
if not wait_heading(mav, 180):
return False
num_circles -= 1
print("Loiter %u circles left" % num_circles)
print("Completed Loiter OK")
return True
def wait_level_flight(mavproxy, mav, accuracy=5, timeout=30):
'''wait for level flight'''
tstart = time.time()
print("Waiting for level flight")
while time.time() < tstart + timeout:
m = mav.recv_match(type='ATTITUDE', blocking=True)
roll = math.degrees(m.roll)
pitch = math.degrees(m.pitch)
print("Roll=%.1f Pitch=%.1f" % (roll, pitch))
if math.fabs(roll) <= accuracy and math.fabs(pitch) <= accuracy:
print("Attained level flight")
return True
print("Failed to attain level flight")
return False
@ -114,7 +127,8 @@ def change_altitude(mavproxy, mav, altitude, accuracy=10):
mavproxy.send('rc 2 2000\n')
else:
mavproxy.send('rc 2 1000\n')
wait_altitude(mav, altitude-accuracy/2, altitude+accuracy/2)
if not wait_altitude(mav, altitude-accuracy/2, altitude+accuracy/2):
return False
mavproxy.send('rc 2 1500\n')
print("Reached target altitude at %u" % mav.messages['VFR_HUD'].alt)
return wait_level_flight(mavproxy, mav)
@ -124,7 +138,8 @@ def axial_left_roll(mavproxy, mav, count=1):
'''fly a left axial roll'''
# full throttle!
mavproxy.send('rc 3 2000\n')
change_altitude(mavproxy, mav, homeloc.alt+200)
if not change_altitude(mavproxy, mav, homeloc.alt+200):
return False
# fly the roll in manual
mavproxy.send('switch 6\n')
@ -133,9 +148,12 @@ def axial_left_roll(mavproxy, mav, count=1):
while count > 0:
print("Starting roll")
mavproxy.send('rc 1 1000\n')
wait_roll(mav, -150, accuracy=20)
wait_roll(mav, 150, accuracy=20)
wait_roll(mav, 0, accuracy=20)
if not wait_roll(mav, -150, accuracy=20):
return False
if not wait_roll(mav, 150, accuracy=20):
return False
if not wait_roll(mav, 0, accuracy=20):
return False
count -= 1
# back to FBWA
@ -150,7 +168,8 @@ def inside_loop(mavproxy, mav, count=1):
'''fly a inside loop'''
# full throttle!
mavproxy.send('rc 3 2000\n')
change_altitude(mavproxy, mav, homeloc.alt+200)
if not change_altitude(mavproxy, mav, homeloc.alt+200):
return False
# fly the loop in manual
mavproxy.send('switch 6\n')
@ -159,8 +178,10 @@ def inside_loop(mavproxy, mav, count=1):
while count > 0:
print("Starting loop")
mavproxy.send('rc 2 1000\n')
wait_pitch(mav, 80, accuracy=20)
wait_pitch(mav, 0, accuracy=20)
if not wait_pitch(mav, 80, accuracy=20):
return False
if not wait_pitch(mav, 0, accuracy=20):
return False
count -= 1
# back to FBWA
@ -183,6 +204,7 @@ def setup_rc(mavproxy):
def fly_mission(mavproxy, mav, filename, height_accuracy=-1, target_altitude=None):
'''fly a mission from a file'''
global homeloc
print("Flying mission %s" % filename)
mavproxy.send('wp load %s\n' % filename)
mavproxy.expect('flight plan received')
mavproxy.send('wp list\n')
@ -287,6 +309,9 @@ def fly_ArduPlane(viewerip=None):
if not fly_RTL(mavproxy, mav):
print("Failed RTL")
failed = True
if not fly_LOITER(mavproxy, mav):
print("Failed LOITER")
failed = True
if not fly_mission(mavproxy, mav, os.path.join(testdir, "ap1.txt"), height_accuracy = 10,
target_altitude=homeloc.alt+100):
print("Failed mission")

2
Tools/autotest/autotest.py
View File

@ -193,7 +193,7 @@ class TestResults(object):
'''test results class'''
def __init__(self):
self.date = time.asctime()
self.githash = util.loadfile(util.reltopdir('.git/refs/heads/master'))
self.githash = util.run_cmd('git rev-parse HEAD', output=True, dir=util.reltopdir('.')).strip()
self.tests = []
self.files = []

16
Tools/autotest/common.py
View File

@ -84,6 +84,7 @@ def wait_altitude(mav, alt_min, alt_max, timeout=30):
if abs(climb_rate) > 0:
tstart = time.time();
if m.alt >= alt_min and m.alt <= alt_max:
print("Altitude OK")
return True
print("Failed to attain altitude range")
return False
@ -110,6 +111,7 @@ def wait_roll(mav, roll, accuracy, timeout=30):
r = math.degrees(m.roll)
print("Roll %u" % r)
if math.fabs(r - roll) <= accuracy:
print("Attained roll %u" % roll)
return True
print("Failed to attain roll %u" % roll)
return False
@ -123,6 +125,7 @@ def wait_pitch(mav, pitch, accuracy, timeout=30):
r = math.degrees(m.pitch)
print("Pitch %u" % r)
if math.fabs(r - pitch) <= accuracy:
print("Attained pitch %u" % pitch)
return True
print("Failed to attain pitch %u" % pitch)
return False
@ -136,6 +139,7 @@ def wait_heading(mav, heading, accuracy=5, timeout=30):
m = mav.recv_match(type='VFR_HUD', blocking=True)
print("Heading %u" % m.heading)
if math.fabs(m.heading - heading) <= accuracy:
print("Attained heading %u" % heading)
return True
print("Failed to attain heading %u" % heading)
return False
@ -151,8 +155,10 @@ def wait_distance(mav, distance, accuracy=5, timeout=30):
delta = get_distance(start, pos)
print("Distance %.2f meters" % delta)
if math.fabs(delta - distance) <= accuracy:
print("Attained distance %.2f meters OK" % delta)
return True
if(delta > (distance + accuracy)):
if delta > (distance + accuracy):
print("Failed distance - overshoot delta=%f distance=%f" % (delta, distance))
return False
print("Failed to attain distance %u" % distance)
return False
@ -163,11 +169,13 @@ def wait_location(mav, loc, accuracy=5, timeout=30, target_altitude=None, height
tstart = time.time()
if target_altitude is None:
target_altitude = loc.alt
print("Waiting for location %.4f,%.4f at altitude %.1f height_accuracy=%.1f" % (
loc.lat, loc.lng, target_altitude, height_accuracy))
while time.time() < tstart + timeout:
m = mav.recv_match(type='GPS_RAW', blocking=True)
pos = current_location(mav)
delta = get_distance(loc, pos)
print("Distance %.2f meters" % delta)
print("Distance %.2f meters alt %.1f" % (delta, pos.alt))
if delta <= accuracy:
if height_accuracy != -1 and math.fabs(pos.alt - target_altitude) > height_accuracy:
continue
@ -212,9 +220,9 @@ def wait_waypoint(mav, wpnum_start, wpnum_end, allow_skip=True, max_dist=2, time
print("Reached final waypoint %u" % seq)
return True
if seq > current_wp+1:
print("Skipped waypoint! Got wp %u expected %u" % (seq, current_wp+1))
print("Failed: Skipped waypoint! Got wp %u expected %u" % (seq, current_wp+1))
return False
print("Timed out waiting for waypoint %u of %u" % (wpnum_end, wpnum_end))
print("Failed: Timed out waiting for waypoint %u of %u" % (wpnum_end, wpnum_end))
return False
def save_wp(mavproxy, mav):

1
Tools/autotest/sim_arducopter.sh
View File

@ -29,6 +29,7 @@ make clean $target
tfile=$(tempfile)
echo r > $tfile
gnome-terminal -e "gdb -x $tfile --args /tmp/ArduCopter.build/ArduCopter.elf"
#gnome-terminal -e "valgrind -q /tmp/ArduCopter.build/ArduCopter.elf"
sleep 2
rm -f $tfile
gnome-terminal -e "../Tools/autotest/pysim/sim_multicopter.py --frame=$frame --home=-35.362938,149.165085,584,270"

1
Tools/autotest/sim_arduplane.sh
View File

@ -10,6 +10,7 @@ make clean sitl
tfile=$(tempfile)
echo r > $tfile
gnome-terminal -e "gdb -x $tfile --args /tmp/ArduPlane.build/ArduPlane.elf"
#gnome-terminal -e "valgrind -q /tmp/ArduPlane.build/ArduPlane.elf"
sleep 2
rm -f $tfile
gnome-terminal -e '../Tools/autotest/jsbsim/runsim.py --home=-35.362938,149.165085,584,270 --wind=5,180,0.2'

18
libraries/AC_PID/AC_PID.cpp
View File

@ -6,6 +6,13 @@
#include <math.h>
#include "AC_PID.h"
const AP_Param::GroupInfo AC_PID::var_info[] PROGMEM = {
AP_GROUPINFO("P", 0, AC_PID, _kp),
AP_GROUPINFO("I", 1, AC_PID, _ki),
AP_GROUPINFO("D", 2, AC_PID, _kd),
AP_GROUPINFO("IMAX", 3, AC_PID, _imax),
AP_GROUPEND
};
int32_t AC_PID::get_p(int32_t error)
{
@ -42,6 +49,7 @@ int32_t AC_PID::get_d(int32_t input, float dt)
// add in derivative component
return _kd * _derivative;
}
return 0;
}
int32_t AC_PID::get_pi(int32_t error, float dt)
@ -108,11 +116,17 @@ AC_PID::reset_I()
void
AC_PID::load_gains()
{
_group.load();
_kp.load();
_ki.load();
_kd.load();
_imax.load();
}
void
AC_PID::save_gains()
{
_group.save();
_kp.save();
_ki.save();
_kd.save();
_imax.save();
}

53
libraries/AC_PID/AC_PID.h
View File

@ -19,55 +19,23 @@ public:
/// @note PIDs must be named to avoid either multiple parameters with the
/// same name, or an overly complex constructor.
///
/// @param key Storage key assigned to this PID. Should be unique.
/// @param name Name by which the PID is known, or NULL for an anonymous PID.
/// The name is prefixed to the P, I, D, IMAX variable names when
/// they are reported.
/// @param initial_p Initial value for the P term.
/// @param initial_i Initial value for the I term.
/// @param initial_d Initial value for the D term.
/// @param initial_imax Initial value for the imax term.4
///
AC_PID(AP_Var::Key key,
const prog_char_t *name,
AC_PID(
const float &initial_p = 0.0,
const float &initial_i = 0.0,
const float &initial_d = 0.0,
const int16_t &initial_imax = 0.0) :
_group(key, name),
// group, index, initial value, name
_kp (&_group, 0, initial_p, PSTR("P")),
_ki (&_group, 1, initial_i, PSTR("I")),
_kd (&_group, 2, initial_d, PSTR("D")),
_imax(&_group, 3, initial_imax, PSTR("IMAX"))
_kp (initial_p),
_ki (initial_i),
_kd (initial_d),
_imax(initial_imax)
{
// no need for explicit load, assuming that the main code uses AP_Var::load_all.
}
/// Constructor for PID that does not save its settings.
///
/// @param name Name by which the PID is known, or NULL for an anonymous PID.
/// The name is prefixed to the P, I, D, IMAX variable names when
/// they are reported.
/// @param initial_p Initial value for the P term.
/// @param initial_i Initial value for the I term.
/// @param initial_d Initial value for the D term.
/// @param initial_imax Initial value for the imax term.4
///
AC_PID(const prog_char_t *name,
const float &initial_p = 0.0,
const float &initial_i = 0.0,
const float &initial_d = 0.0,
const int16_t &initial_imax = 0.0) :
_group(AP_Var::k_key_none, name),
// group, index, initial value, name
_kp (&_group, 0, initial_p, PSTR("P")),
_ki (&_group, 1, initial_i, PSTR("I")),
_kd (&_group, 2, initial_d, PSTR("D")),
_imax(&_group, 3, initial_imax, PSTR("IMAX"))
{
// no need for explicit load, assuming that the main code uses AP_Param::load_all.
}
/// Iterate the PID, return the new control value
@ -125,11 +93,12 @@ public:
float get_integrator() const { return _integrator; }
static const struct AP_Param::GroupInfo var_info[];
private:
AP_Var_group _group;
AP_Float16 _kp;
AP_Float16 _ki;
AP_Float16 _kd;
AP_Float _kp;
AP_Float _ki;
AP_Float _kd;
AP_Int16 _imax;
float _integrator; ///< integrator value

15
libraries/APM_PI/APM_PI.cpp
View File

@ -7,6 +7,13 @@
#include "APM_PI.h"
const AP_Param::GroupInfo APM_PI::var_info[] PROGMEM = {
AP_GROUPINFO("P", 0, APM_PI, _kp),
AP_GROUPINFO("I", 1, APM_PI, _ki),
AP_GROUPINFO("IMAX", 2, APM_PI, _imax),
AP_GROUPEND
};
int32_t APM_PI::get_p(int32_t error)
{
return (float)error * _kp;
@ -40,11 +47,15 @@ APM_PI::reset_I()
void
APM_PI::load_gains()
{
_group.load();
_kp.load();
_ki.load();
_imax.load();
}
void
APM_PI::save_gains()
{
_group.save();
_kp.save();
_ki.save();
_imax.save();
}

50
libraries/APM_PI/APM_PI.h
View File

@ -19,51 +19,20 @@ public:
/// @note PI must be named to avoid either multiple parameters with the
/// same name, or an overly complex constructor.
///
/// @param key Storage key assigned to this PI. Should be unique.
/// @param name Name by which the PI is known, or NULL for an anonymous PI.
/// The name is prefixed to the P, I, IMAX variable names when
/// they are reported.
/// @param initial_p Initial value for the P term.
/// @param initial_i Initial value for the I term.
/// @param initial_imax Initial value for the imax term.4
///
APM_PI(AP_Var::Key key,
const prog_char_t *name,
const float &initial_p = 0.0,
const float &initial_i = 0.0,
const int16_t &initial_imax = 0.0) :
_group(key, name),
// group, index, initial value, name
_kp (&_group, 0, initial_p, PSTR("P")),
_ki (&_group, 1, initial_i, PSTR("I")),
_imax(&_group, 3, initial_imax, PSTR("IMAX"))
APM_PI(const float &initial_p = 0.0,
const float &initial_i = 0.0,
const int16_t &initial_imax = 0.0) :
_kp (initial_p),
_ki (initial_i),
_imax(initial_imax)
{
// no need for explicit load, assuming that the main code uses AP_Var::load_all.
}
/// Constructor for PI that does not save its settings.
///
/// @param name Name by which the PI is known, or NULL for an anonymous PI.
/// The name is prefixed to the P, I, IMAX variable names when
/// they are reported.
/// @param initial_p Initial value for the P term.
/// @param initial_i Initial value for the I term.
/// @param initial_imax Initial value for the imax term.4
///
APM_PI(const prog_char_t *name,
const float &initial_p = 0.0,
const float &initial_i = 0.0,
const int16_t &initial_imax = 0.0) :
_group(AP_Var::k_key_none, name),
// group, index, initial value, name
_kp (&_group, 0, initial_p, PSTR("P")),
_ki (&_group, 1, initial_i, PSTR("I")),
_imax(&_group, 3, initial_imax, PSTR("IMAX"))
{
}
/// Iterate the PI, return the new control value
///
/// Positive error produces positive output.
@ -115,10 +84,11 @@ public:
float get_integrator() const { return _integrator; }
void set_integrator(float i) { _integrator = i; }
static const struct AP_Param::GroupInfo var_info[];
private:
AP_Var_group _group;
AP_Float16 _kp;
AP_Float16 _ki;
AP_Float _kp;
AP_Float _ki;
AP_Int16 _imax;
float _integrator; ///< integrator value

8
libraries/APO/Navigator_Dcm.cpp
View File

@ -99,7 +99,7 @@ void Navigator_Dcm::calibrate() {
}
flashcount++;
}
_groundPressure.save();
_groundTemperature.save();
@ -133,10 +133,10 @@ void Navigator_Dcm::updateFast(float dt) {
* temp input is in deg C *10
*/
_board->baro->Read(); // Get new data from absolute pressure sensor
float reference = 44330 * (1.0 - (pow(_groundPressure.get()/101325.0,0.190295)));
setAlt(_baroLowPass.update((44330 * (1.0 - (pow((_board->baro->Press/101325.0),0.190295)))) - reference,dt));
float reference = 44330.0 * (1.0 - (pow(_groundPressure.get()/101325.0,0.190295)));
setAlt(_baroLowPass.update((44330.0 * (1.0 - (pow((_board->baro->Press/101325.0),0.190295)))) - reference,dt));
//_board->debug->printf_P(PSTR("Ground Pressure %f\tAltitude = %f\tGround Temperature = %f\tPress = %ld\tTemp = %d\n"),_groundPressure.get(),getAlt(),_groundTemperature.get(),_board->baro->Press,_board->baro->Temp);
// last resort, use gps altitude
} else if (_board->gps && _board->gps->fix) {
setAlt_intM(_board->gps->altitude * 10); // gps in cm, intM in mm

54
libraries/AP_Baro/AP_Baro_MS5611.cpp
View File

@ -21,7 +21,7 @@
read() : Read sensor data and _calculate Temperature, Pressure and Altitude
This function is optimized so the main host don´t need to wait
You can call this function in your main loop
Maximun data output frequency 100Hz
Maximum data output frequency 100Hz - this allows maximum oversampling in the chip ADC
It returns a 1 if there are new data.
get_pressure() : return pressure in mbar*100 units
get_temperature() : return temperature in celsius degrees*100 units
@ -49,13 +49,13 @@
#define CMD_MS5611_PROM_C5 0xAA
#define CMD_MS5611_PROM_C6 0xAC
#define CMD_MS5611_PROM_CRC 0xAE
#define CMD_CONVERT_D1_OSR4096 0x48 // Maximun resolution
#define CMD_CONVERT_D2_OSR4096 0x58 // Maximun resolution
#define CMD_CONVERT_D1_OSR4096 0x48 // Maximum resolution (oversampling)
#define CMD_CONVERT_D2_OSR4096 0x58 // Maximum resolution (oversampling)
uint32_t AP_Baro_MS5611::_s_D1;
uint32_t AP_Baro_MS5611::_s_D2;
uint8_t AP_Baro_MS5611::_state;
long AP_Baro_MS5611::_timer;
uint32_t AP_Baro_MS5611::_timer;
bool AP_Baro_MS5611::_sync_access;
bool AP_Baro_MS5611::_updated;
@ -116,11 +116,12 @@ bool AP_Baro_MS5611::init( AP_PeriodicProcess *scheduler )
pinMode(MS5611_CS, OUTPUT); // Chip select Pin
digitalWrite(MS5611_CS, HIGH);
delay(1);
_spi_write(CMD_MS5611_RESET);
delay(4);
// We read the factory calibration
// The on-chip CRC is not used
C1 = _spi_read_16bits(CMD_MS5611_PROM_C1);
C2 = _spi_read_16bits(CMD_MS5611_PROM_C2);
C3 = _spi_read_16bits(CMD_MS5611_PROM_C3);
@ -135,7 +136,7 @@ bool AP_Baro_MS5611::init( AP_PeriodicProcess *scheduler )
_state = 1;
Temp=0;
Press=0;
scheduler->register_process( AP_Baro_MS5611::_update );
healthy = true;
@ -150,12 +151,15 @@ void AP_Baro_MS5611::_update(uint32_t tnow)
{
if (_sync_access) return;
if (tnow - _timer < 10000) {
return; // wait for more than 10ms
// Throttle read rate to 100hz maximum.
// note we use 9500us here not 10000us
// the read rate will end up at exactly 100hz because the Periodic Timer fires at 1khz
if (tnow - _timer < 9500) {
return;
}
_timer = tnow;
if (_state == 1) {
_s_D2 = _spi_read_adc(); // On state 1 we read temp
_state++;
@ -193,30 +197,30 @@ uint8_t AP_Baro_MS5611::read()
void AP_Baro_MS5611::_calculate()
{
int32_t dT;
long long TEMP; // 64 bits
long long OFF;
long long SENS;
long long P;
int64_t TEMP; // 64 bits
int64_t OFF;
int64_t SENS;
int64_t P;
// Formulas from manufacturer datasheet
// TODO: optimization with shift operations... (shift operations works well on 64 bits variables?)
// We define parameters as 64 bits to prevent overflow on operations
// as per data sheet some intermediate results require over 32 bits, therefore
// we define parameters as 64 bits to prevent overflow on operations
// sub -20c temperature compensation is not included
dT = D2-((long)C5*256);
TEMP = 2000 + ((long long)dT * C6)/8388608;
OFF = (long long)C2 * 65536 + ((long long)C4 * dT ) / 128;
SENS = (long long)C1 * 32768 + ((long long)C3 * dT) / 256;
TEMP = 2000 + ((int64_t)dT * C6)/8388608;
OFF = (int64_t)C2 * 65536 + ((int64_t)C4 * dT ) / 128;
SENS = (int64_t)C1 * 32768 + ((int64_t)C3 * dT) / 256;
/*
if (TEMP < 2000){ // second order temperature compensation
long long T2 = (long long)dT*dT / 2147483648;
long long Aux_64 = (TEMP-2000)*(TEMP-2000);
long long OFF2 = 5*Aux_64/2;
long long SENS2 = 5*Aux_64/4;
int64_t T2 = (((int64_t)dT)*dT) / 2147483648UL;
int64_t Aux_64 = (TEMP-2000)*(TEMP-2000);
int64_t OFF2 = 5*Aux_64/2;
int64_t SENS2 = 5*Aux_64/4;
TEMP = TEMP - T2;
OFF = OFF - OFF2;
SENS = SENS - SENS2;
}
*/
P = (D1*SENS/2097152 - OFF)/32768;
Temp = TEMP;
Press = P;
@ -241,7 +245,7 @@ float AP_Baro_MS5611::get_altitude()
tmp_float = (Press / 101325.0);
tmp_float = pow(tmp_float, 0.190295);
Altitude = 44330 * (1.0 - tmp_float);
Altitude = 44330.0 * (1.0 - tmp_float);
return (Altitude);
}

2
libraries/AP_Baro/AP_Baro_MS5611.h
View File

@ -27,7 +27,7 @@ class AP_Baro_MS5611 : public AP_Baro
static bool _updated;
static uint32_t _s_D1, _s_D2;
static uint8_t _state;
static long _timer;
static uint32_t _timer;
/* Gates access to asynchronous state: */
static bool _sync_access;

6
libraries/AP_Baro/examples/AP_Baro_BMP085_test/AP_Baro_BMP085_test.pde
View File

@ -28,7 +28,7 @@ Arduino_Mega_ISR_Registry isr_registry;
AP_TimerProcess scheduler;
void setup()
{
{
Serial.begin(115200);
Serial.println("ArduPilot Mega BMP085 library test");
Serial.println("Initialising barometer..."); delay(100);
@ -53,7 +53,7 @@ void loop()
{
float tmp_float;
float Altitude;
if((micros()- timer) > 50000L){
timer = micros();
APM_BMP085.read();
@ -69,7 +69,7 @@ void loop()
Serial.print(" Altitude:");
tmp_float = (APM_BMP085.get_pressure() / 101325.0);
tmp_float = pow(tmp_float, 0.190295);
Altitude = 44330 * (1.0 - tmp_float);
Altitude = 44330.0 * (1.0 - tmp_float);
Serial.print(Altitude);
Serial.printf(" t=%u", (unsigned)read_time);
Serial.println();

6
libraries/AP_Baro/examples/AP_Baro_MS5611_test/AP_Baro_MS5611_test.pde
View File

@ -18,7 +18,7 @@ AP_TimerProcess scheduler;
unsigned long timer;
void setup()
{
{
Serial.begin(115200, 128, 128);
Serial.println("ArduPilot Mega MeasSense Barometer library test");
@ -40,7 +40,7 @@ void loop()
{
float tmp_float;
float Altitude;
if((micros()- timer) > 50000L){
timer = micros();
baro.read();
@ -56,7 +56,7 @@ void loop()
Serial.print(" Altitude:");
tmp_float = (baro.get_pressure() / 101325.0);
tmp_float = pow(tmp_float, 0.190295);
Altitude = 44330 * (1.0 - tmp_float);
Altitude = 44330.0 * (1.0 - tmp_float);
Serial.print(Altitude);
Serial.printf(" t=%u", (unsigned)read_time);
Serial.println();

14
libraries/AP_Common/AP_Common.h
View File

@ -38,9 +38,13 @@ typedef struct {
#include "c++.h" // c++ additions
//#include "AP_Vector.h"
//#include "AP_Loop.h"
// default to AP_Param system, unless USE_AP_VAR is defined
#ifdef USE_AP_VAR
#include "AP_Var.h"
#else
#include "AP_Param.h"
#endif
////////////////////////////////////////////////////////////////////////////////
/// @name Warning control
@ -223,5 +227,11 @@ struct Location {
//@}
#ifdef DESKTOP_BUILD
// used to report serious errors in autotest
# define SITL_debug(fmt, args...) fprintf(stdout, "%s:%u " fmt, __FUNCTION__, __LINE__, ##args)
#else
# define SITL_debug(fmt, args...)
#endif
#endif // _AP_COMMON_H

725
libraries/AP_Common/AP_Param.cpp Normal file
View File

@ -0,0 +1,725 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
//
// This 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.
//
// total up and check overflow
// check size of group var_info
/// @file AP_Param.cpp
/// @brief The AP variable store.
#include <FastSerial.h>
#include <AP_Common.h>
#include <math.h>
#include <string.h>
// #define ENABLE_FASTSERIAL_DEBUG
#ifdef ENABLE_FASTSERIAL_DEBUG
# define serialDebug(fmt, args...) if (FastSerial::getInitialized(0)) do {Serial.printf("%s:%d: " fmt "\n", __FUNCTION__, __LINE__ , ##args); delay(0); } while(0)
#else
# define serialDebug(fmt, args...)
#endif
// some useful progmem macros
#define PGM_UINT8(addr) pgm_read_byte((const prog_char *)addr)
#define PGM_UINT16(addr) pgm_read_word((const uint16_t *)addr)
#define PGM_POINTER(addr) pgm_read_pointer((const void *)addr)
// the 'GROUP_ID' of a element of a group is the 8 bit identifier used
// to distinguish between this element of the group and other elements
// of the same group. It is calculated using a bit shift per level of
// nesting, so the first level of nesting gets 4 bits, and the next
// level gets the next 4 bits. This limits groups to having at most 16
// elements.
#define GROUP_ID(grpinfo, base, i, shift) ((base)+(((uint16_t)PGM_UINT8(&grpinfo[i].idx))<<(shift)))
// Static member variables for AP_Param.
//
// max EEPROM write size. This is usually less than the physical
// size as only part of the EEPROM is reserved for parameters
uint16_t AP_Param::_eeprom_size;
// number of rows in the _var_info[] table
uint8_t AP_Param::_num_vars;
// storage and naming information about all types that can be saved
const AP_Param::Info *AP_Param::_var_info;
// write to EEPROM, checking each byte to avoid writing
// bytes that are already correct
void AP_Param::eeprom_write_check(const void *ptr, uint16_t ofs, uint8_t size)
{
const uint8_t *b = (const uint8_t *)ptr;
while (size--) {
uint8_t v = eeprom_read_byte((const uint8_t *)ofs);
if (v != *b) {
eeprom_write_byte((uint8_t *)ofs, *b);
}
b++;
ofs++;
}
}
// write a sentinal value at the given offset
void AP_Param::write_sentinal(uint16_t ofs)
{
struct Param_header phdr;
phdr.type = _sentinal_type;
phdr.key = _sentinal_key;
phdr.group_element = _sentinal_group;
eeprom_write_check(&phdr, ofs, sizeof(phdr));
}
// erase all EEPROM variables by re-writing the header and adding
// a sentinal
void AP_Param::erase_all(void)
{
struct EEPROM_header hdr;
serialDebug("erase_all");
// write the header
hdr.magic[0] = k_EEPROM_magic0;
hdr.magic[1] = k_EEPROM_magic1;
hdr.revision = k_EEPROM_revision;
hdr.spare = 0;
eeprom_write_check(&hdr, 0, sizeof(hdr));
// add a sentinal directly after the header
write_sentinal(sizeof(struct EEPROM_header));
}
// validate a group info table
bool AP_Param::check_group_info(const struct AP_Param::GroupInfo *group_info,
uint16_t *total_size,
uint8_t group_shift)
{
uint8_t type;
int8_t max_idx = -1;
for (uint8_t i=0;
(type=PGM_UINT8(&group_info[i].type)) != AP_PARAM_NONE;
i++) {
if (type == AP_PARAM_GROUP) {
// a nested group
const struct GroupInfo *ginfo = (const struct GroupInfo *)PGM_POINTER(&group_info[i].group_info);
if (group_shift + _group_level_shift >= _group_bits) {
// double nesting of groups is not allowed
return false;
}
if (ginfo == NULL ||
!check_group_info(ginfo, total_size, group_shift + _group_level_shift)) {
return false;
}
continue;
}
uint8_t idx = PGM_UINT8(&group_info[i].idx);
if (idx >= (1<<_group_level_shift)) {
// passed limit on table size
return false;
}
if ((int8_t)idx <= max_idx) {
// the indexes must be in increasing order
return false;
}
max_idx = (int8_t)idx;
uint8_t size = type_size((enum ap_var_type)type);
if (size == 0) {
// not a valid type
return false;
}
(*total_size) += size + sizeof(struct Param_header);
}
return true;
}
// validate the _var_info[] table
bool AP_Param::check_var_info(void)
{
uint16_t total_size = sizeof(struct EEPROM_header);
for (uint8_t i=0; i<_num_vars; i++) {
uint8_t type = PGM_UINT8(&_var_info[i].type);
if (type == AP_PARAM_GROUP) {
if (i == 0) {
// first element can't be a group, for first() call
return false;
}
const struct GroupInfo *group_info = (const struct GroupInfo *)PGM_POINTER(&_var_info[i].group_info);
if (group_info == NULL ||
!check_group_info(group_info, &total_size, 0)) {
return false;
}
} else {
uint8_t size = type_size((enum ap_var_type)type);
if (size == 0) {
// not a valid type - the top level list can't contain
// AP_PARAM_NONE
return false;
}
total_size += size + sizeof(struct Param_header);
}
}
if (total_size > _eeprom_size) {
serialDebug("total_size %u exceeds _eeprom_size %u",
total_size, _eeprom_size);
return false;
}
return true;
}
// setup the _var_info[] table
bool AP_Param::setup(const AP_Param::Info *info, uint8_t num_vars, uint16_t eeprom_size)
{
struct EEPROM_header hdr;
_eeprom_size = eeprom_size;
_var_info = info;
_num_vars = num_vars;
if (!check_var_info()) {
return false;
}
serialDebug("setup %u vars", (unsigned)num_vars);
// check the header
eeprom_read_block(&hdr, 0, sizeof(hdr));
if (hdr.magic[0] != k_EEPROM_magic0 ||
hdr.magic[1] != k_EEPROM_magic1 ||
hdr.revision != k_EEPROM_revision) {
// header doesn't match. We can't recover any variables. Wipe
// the header and setup the sentinal directly after the header
serialDebug("bad header in setup - erasing");
erase_all();
}
return true;
}
// find the info structure given a header and a group_info table
// return the Info structure and a pointer to the variables storage
const struct AP_Param::Info *AP_Param::find_by_header_group(struct Param_header phdr, void **ptr,
uint8_t vindex,
const struct GroupInfo *group_info,
uint8_t group_base,
uint8_t group_shift)
{
uint8_t type;
for (uint8_t i=0;
(type=PGM_UINT8(&group_info[i].type)) != AP_PARAM_NONE;
i++) {
if (type == AP_PARAM_GROUP) {
// a nested group
if (group_shift + _group_level_shift >= _group_bits) {
// too deeply nested - this should have been caught by
// setup() !
return NULL;
}
const struct GroupInfo *ginfo = (const struct GroupInfo *)PGM_POINTER(&group_info[i].group_info);
const struct AP_Param::Info *ret = find_by_header_group(phdr, ptr, vindex, ginfo,
GROUP_ID(group_info, group_base, i, group_shift),
group_shift + _group_level_shift);
if (ret != NULL) {
return ret;
}
continue;
}
if (GROUP_ID(group_info, group_base, i, group_shift) == phdr.group_element) {
// found a group element
*ptr = (void*)(PGM_POINTER(&_var_info[vindex].ptr) + PGM_UINT16(&group_info[i].offset));
return &_var_info[vindex];
}
}
return NULL;
}
// find the info structure given a header
// return the Info structure and a pointer to the variables storage
const struct AP_Param::Info *AP_Param::find_by_header(struct Param_header phdr, void **ptr)
{
// loop over all named variables
for (uint8_t i=0; i<_num_vars; i++) {
uint8_t type = PGM_UINT8(&_var_info[i].type);
uint8_t key = PGM_UINT8(&_var_info[i].key);
if (key != phdr.key) {
// not the right key
continue;
}
if (type != AP_PARAM_GROUP) {
// if its not a group then we are done
*ptr = (void*)PGM_POINTER(&_var_info[i].ptr);
return &_var_info[i];
}
const struct GroupInfo *group_info = (const struct GroupInfo *)PGM_POINTER(&_var_info[i].group_info);
return find_by_header_group(phdr, ptr, i, group_info, 0, 0);
}
return NULL;
}
// find the info structure for a variable in a group
const struct AP_Param::Info *AP_Param::find_var_info_group(const struct GroupInfo *group_info,
uint8_t vindex,
uint8_t group_base,
uint8_t group_shift,
uint8_t *group_element,
const struct GroupInfo **group_ret)
{
uintptr_t base = PGM_POINTER(&_var_info[vindex].ptr);
uint8_t type;
for (uint8_t i=0;
(type=PGM_UINT8(&group_info[i].type)) != AP_PARAM_NONE;
i++) {
if (type == AP_PARAM_GROUP) {
const struct GroupInfo *ginfo = (const struct GroupInfo *)PGM_POINTER(&group_info[i].group_info);
// a nested group
if (group_shift + _group_level_shift >= _group_bits) {
// too deeply nested - this should have been caught by
// setup() !
return NULL;
}
const struct AP_Param::Info *info;
info = find_var_info_group(ginfo, vindex,
GROUP_ID(group_info, group_base, i, group_shift),
group_shift + _group_level_shift,
group_element,
group_ret);
if (info != NULL) {
return info;
}
} else if ((uintptr_t)this == base + PGM_POINTER(&group_info[i].offset)) {
*group_element = GROUP_ID(group_info, group_base, i, group_shift);
*group_ret = &group_info[i];
return &_var_info[vindex];
}
}
return NULL;
}
// find the info structure for a variable
const struct AP_Param::Info *AP_Param::find_var_info(uint8_t *group_element,
const struct GroupInfo **group_ret)
{
for (uint8_t i=0; i<_num_vars; i++) {
uint8_t type = PGM_UINT8(&_var_info[i].type);
uintptr_t base = PGM_POINTER(&_var_info[i].ptr);
if (type == AP_PARAM_GROUP) {
const struct GroupInfo *group_info = (const struct GroupInfo *)PGM_POINTER(&_var_info[i].group_info);
const struct AP_Param::Info *info;
info = find_var_info_group(group_info, i, 0, 0, group_element, group_ret);
if (info != NULL) {
return info;
}
} else if (base == (uintptr_t)this) {
*group_element = 0;
*group_ret = NULL;
return &_var_info[i];
}
}
return NULL;
}
// return the storage size for a AP_PARAM_* type
const uint8_t AP_Param::type_size(enum ap_var_type type)
{
switch (type) {
case AP_PARAM_NONE:
case AP_PARAM_GROUP:
return 0;
case AP_PARAM_INT8:
return 1;
case AP_PARAM_INT16:
return 2;
case AP_PARAM_INT32:
return 4;
case AP_PARAM_FLOAT:
return 4;
case AP_PARAM_VECTOR3F:
return 3*4;
case AP_PARAM_VECTOR6F:
return 6*4;
case AP_PARAM_MATRIX3F:
return 3*3*4;
}
serialDebug("unknown type %u\n", type);
return 0;
}
// scan the EEPROM looking for a given variable by header content
// return true if found, along with the offset in the EEPROM where
// the variable is stored
// if not found return the offset of the sentinal, or
bool AP_Param::scan(const AP_Param::Param_header *target, uint16_t *pofs)
{
struct Param_header phdr;
uint16_t ofs = sizeof(AP_Param::EEPROM_header);
while (ofs < _eeprom_size) {
eeprom_read_block(&phdr, (void *)ofs, sizeof(phdr));
if (phdr.type == target->type &&
phdr.key == target->key &&
phdr.group_element == target->group_element) {
// found it
*pofs = ofs;
return true;
}
// note that this is an ||, not an &&, as this makes us more
// robust to power off while adding a variable to EEPROM
if (phdr.type == _sentinal_type ||
phdr.key == _sentinal_key ||
phdr.group_element == _sentinal_group) {
// we've reached the sentinal
*pofs = ofs;
return false;
}
ofs += type_size((enum ap_var_type)phdr.type) + sizeof(phdr);
}
*pofs = ~0;
serialDebug("scan past end of eeprom");
return false;
}
// Copy the variable's whole name to the supplied buffer.
//
// If the variable is a group member, prepend the group name.
//
void AP_Param::copy_name(char *buffer, size_t buffer_size)
{
uint8_t group_element;
const struct GroupInfo *ginfo;
const struct AP_Param::Info *info = find_var_info(&group_element, &ginfo);
if (info == NULL) {
*buffer = 0;
serialDebug("no info found");
return;
}
strncpy_P(buffer, info->name, buffer_size);
if (ginfo != NULL) {
uint8_t len = strnlen(buffer, buffer_size);
if (len < buffer_size) {
strncpy_P(&buffer[len], ginfo->name, buffer_size-len);
}
}
}
// Find a variable by name in a group
AP_Param *
AP_Param::find_group(const char *name, uint8_t vindex, const struct GroupInfo *group_info, enum ap_var_type *ptype)
{
uint8_t type;
for (uint8_t i=0;
(type=PGM_UINT8(&group_info[i].type)) != AP_PARAM_NONE;
i++) {
if (type == AP_PARAM_GROUP) {
const struct GroupInfo *ginfo = (const struct GroupInfo *)PGM_POINTER(&group_info[i].group_info);
AP_Param *ap = find_group(name, vindex, ginfo, ptype);
if (ap != NULL) {
return ap;
}
} else if (strcasecmp_P(name, group_info[i].name) == 0) {
uintptr_t p = PGM_POINTER(&_var_info[vindex].ptr);
*ptype = (enum ap_var_type)type;
return (AP_Param *)(p + PGM_POINTER(&group_info[i].offset));
}
}
return NULL;
}
// Find a variable by name.
//
AP_Param *
AP_Param::find(const char *name, enum ap_var_type *ptype)
{
for (uint8_t i=0; i<_num_vars; i++) {
uint8_t type = PGM_UINT8(&_var_info[i].type);
if (type == AP_PARAM_GROUP) {
uint8_t len = strnlen_P(_var_info[i].name, AP_MAX_NAME_SIZE);
if (strncmp_P(name, _var_info[i].name, len) != 0) {
continue;
}
const struct GroupInfo *group_info = (const struct GroupInfo *)PGM_POINTER(&_var_info[i].group_info);
return find_group(name + len, i, group_info, ptype);
} else if (strcasecmp_P(name, _var_info[i].name) == 0) {
*ptype = (enum ap_var_type)type;
return (AP_Param *)PGM_POINTER(&_var_info[i].ptr);
}
}
return NULL;
}
// Save the variable to EEPROM, if supported
//
bool AP_Param::save(void)
{
uint8_t group_element = 0;
const struct GroupInfo *ginfo;
const struct AP_Param::Info *info = find_var_info(&group_element, &ginfo);
if (info == NULL) {
// we don't have any info on how to store it
return false;
}
struct Param_header phdr;
// create the header we will use to store the variable
if (ginfo != NULL) {
phdr.type = PGM_UINT8(&ginfo->type);
} else {
phdr.type = PGM_UINT8(&info->type);
}
phdr.key = PGM_UINT8(&info->key);
phdr.group_element = group_element;
// scan EEPROM to find the right location
uint16_t ofs;
if (scan(&phdr, &ofs)) {
// found an existing copy of the variable
eeprom_write_check(this, ofs+sizeof(phdr), type_size((enum ap_var_type)phdr.type));
return true;
}
if (ofs == (uint16_t)~0) {
return false;
}
// write a new sentinal, then the data, then the header
write_sentinal(ofs + sizeof(phdr) + type_size((enum ap_var_type)phdr.type));
eeprom_write_check(this, ofs+sizeof(phdr), type_size((enum ap_var_type)phdr.type));
eeprom_write_check(&phdr, ofs, sizeof(phdr));
return true;
}
// Load the variable from EEPROM, if supported
//
bool AP_Param::load(void)
{
uint8_t group_element = 0;
const struct GroupInfo *ginfo;
const struct AP_Param::Info *info = find_var_info(&group_element, &ginfo);
if (info == NULL) {
// we don't have any info on how to load it
return false;
}
struct Param_header phdr;
// create the header we will use to match the variable
if (ginfo != NULL) {
phdr.type = PGM_UINT8(&ginfo->type);
} else {
phdr.type = PGM_UINT8(&info->type);
}
phdr.key = PGM_UINT8(&info->key);
phdr.group_element = group_element;
// scan EEPROM to find the right location
uint16_t ofs;
if (!scan(&phdr, &ofs)) {
return false;
}
// found it
eeprom_read_block(this, (void*)(ofs+sizeof(phdr)), type_size((enum ap_var_type)phdr.type));
return true;
}
// Load all variables from EEPROM
//
bool AP_Param::load_all(void)
{
struct Param_header phdr;
uint16_t ofs = sizeof(AP_Param::EEPROM_header);
while (ofs < _eeprom_size) {
eeprom_read_block(&phdr, (void *)ofs, sizeof(phdr));
// note that this is an || not an && for robustness
// against power off while adding a variable
if (phdr.type == _sentinal_type ||
phdr.key == _sentinal_key ||
phdr.group_element == _sentinal_group) {
// we've reached the sentinal
return true;
}
const struct AP_Param::Info *info;
void *ptr;
info = find_by_header(phdr, &ptr);
if (info != NULL) {
eeprom_read_block(ptr, (void*)(ofs+sizeof(phdr)), type_size((enum ap_var_type)phdr.type));
}
ofs += type_size((enum ap_var_type)phdr.type) + sizeof(phdr);
}
// we didn't find the sentinal
serialDebug("no sentinal in load_all");
return false;
}
// return the first variable in _var_info
AP_Param *AP_Param::first(uint16_t *token, enum ap_var_type *ptype)
{
*token = 0;
if (_num_vars == 0) {
return NULL;
}
if (ptype != NULL) {
*ptype = (enum ap_var_type)PGM_UINT8(&_var_info[0].type);
}
return (AP_Param *)(PGM_POINTER(&_var_info[0].ptr));
}
/// Returns the next variable in a group, recursing into groups
/// as needed
AP_Param *AP_Param::next_group(uint8_t vindex, const struct GroupInfo *group_info,
bool *found_current,
uint8_t group_base,
uint8_t group_shift,
uint16_t *token,
enum ap_var_type *ptype)
{
uint8_t type;
for (uint8_t i=0;
(type=PGM_UINT8(&group_info[i].type)) != AP_PARAM_NONE;
i++) {
if (type == AP_PARAM_GROUP) {
// a nested group
const struct GroupInfo *ginfo = (const struct GroupInfo *)PGM_POINTER(&group_info[i].group_info);
AP_Param *ap;
ap = next_group(vindex, ginfo, found_current, GROUP_ID(group_info, group_base, i, group_shift),
group_shift + _group_level_shift, token, ptype);
if (ap != NULL) {
return ap;
}
} else {
if (*found_current) {
// got a new one
(*token) = ((uint16_t)GROUP_ID(group_info, group_base, i, group_shift)<<8) | vindex;
if (ptype != NULL) {
*ptype = (enum ap_var_type)type;
}
return (AP_Param*)(PGM_POINTER(&_var_info[vindex].ptr) + PGM_UINT16(&group_info[i].offset));
}
if (GROUP_ID(group_info, group_base, i, group_shift) == (uint8_t)((*token)>>8)) {
*found_current = true;
}
}
}
return NULL;
}
/// Returns the next variable in _var_info, recursing into groups
/// as needed
AP_Param *AP_Param::next(uint16_t *token, enum ap_var_type *ptype)
{
uint8_t i = (*token)&0xFF;
bool found_current = false;
if (i >= _num_vars) {
// illegal token
return NULL;
}
uint8_t type = PGM_UINT8(&_var_info[i].type);
if (type != AP_PARAM_GROUP) {
i++;
found_current = true;
}
for (; i<_num_vars; i++) {
type = PGM_UINT8(&_var_info[i].type);
if (type == AP_PARAM_GROUP) {
const struct GroupInfo *group_info = (const struct GroupInfo *)PGM_POINTER(&_var_info[i].group_info);
AP_Param *ap = next_group(i, group_info, &found_current, 0, 0, token, ptype);
if (ap != NULL) {
return ap;
}
} else {
// found the next one
(*token) = i;
if (ptype != NULL) {
*ptype = (enum ap_var_type)type;
}
return (AP_Param *)(PGM_POINTER(&_var_info[i].ptr));
}
}
return NULL;
}
/// Returns the next scalar in _var_info, recursing into groups
/// as needed
AP_Param *AP_Param::next_scalar(uint16_t *token, enum ap_var_type *ptype)
{
AP_Param *ap;
enum ap_var_type type;
while ((ap = next(token, &type)) != NULL && type > AP_PARAM_FLOAT) ;
if (ap != NULL && ptype != NULL) {
*ptype = type;
}
return ap;
}
/// cast a variable to a float given its type
float AP_Param::cast_to_float(enum ap_var_type type)
{
switch (type) {
case AP_PARAM_INT8:
return ((AP_Int8 *)this)->cast_to_float();
case AP_PARAM_INT16:
return ((AP_Int16 *)this)->cast_to_float();
case AP_PARAM_INT32:
return ((AP_Int32 *)this)->cast_to_float();
case AP_PARAM_FLOAT:
return ((AP_Float *)this)->cast_to_float();
default:
return NAN;
}
}
// print the value of all variables
void AP_Param::show_all(void)
{
uint16_t token;
AP_Param *ap;
enum ap_var_type type;
for (ap=AP_Param::first(&token, &type);
ap;
ap=AP_Param::next(&token, &type)) {
char s[AP_MAX_NAME_SIZE+1];
ap->copy_name(s, sizeof(s));
s[AP_MAX_NAME_SIZE] = 0;
switch (type) {
case AP_PARAM_INT8:
Serial.printf_P(PSTR("%s: %d\n"), s, (int)((AP_Int8 *)ap)->get());
break;
case AP_PARAM_INT16:
Serial.printf_P(PSTR("%s: %d\n"), s, (int)((AP_Int16 *)ap)->get());
break;
case AP_PARAM_INT32:
Serial.printf_P(PSTR("%s: %ld\n"), s, (long)((AP_Int32 *)ap)->get());
break;
case AP_PARAM_FLOAT:
Serial.printf_P(PSTR("%s: %f\n"), s, ((AP_Float *)ap)->get());
break;
case AP_PARAM_VECTOR3F: {
Vector3f v = ((AP_Vector3f *)ap)->get();
Serial.printf_P(PSTR("%s: %f %f %f\n"), s, v.x, v.y, v.z);
break;
}
default:
break;
}
}
}

432
libraries/AP_Common/AP_Param.h Normal file
View File

@ -0,0 +1,432 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
//
// This 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.
//
/// @file AP_Param.h
/// @brief A system for managing and storing variables that are of
/// general interest to the system.
#ifndef AP_PARAM_H
#define AP_PARAM_H
#include <stddef.h>
#include <string.h>
#include <stdint.h>
#include <avr/pgmspace.h>
#include <avr/eeprom.h>
#include <AP_Math.h>
#define AP_MAX_NAME_SIZE 15
// a varient of offsetof() to work around C++ restrictions.
// this can only be used when the offset of a variable in a object
// is constant and known at compile time
#define AP_VAROFFSET(type, element) (((uintptr_t)(&((const type *)1)->element))-1)
// find the type of a variable given the class and element
#define AP_CLASSTYPE(class, element) (((const class *)1)->element.vtype)
// declare a group var_info line
#define AP_GROUPINFO(name, idx, class, element) { AP_CLASSTYPE(class, element), idx, name, AP_VAROFFSET(class, element) }
// declare a nested group entry in a group var_info
#define AP_NESTEDGROUPINFO(class, idx) { AP_PARAM_GROUP, idx, "", 0, class::var_info }
#define AP_GROUPEND { AP_PARAM_NONE, 0xFF, "" }
enum ap_var_type {
AP_PARAM_NONE = 0,
AP_PARAM_INT8,
AP_PARAM_INT16,
AP_PARAM_INT32,
AP_PARAM_FLOAT,
AP_PARAM_VECTOR3F,
AP_PARAM_VECTOR6F,
AP_PARAM_MATRIX3F,
AP_PARAM_GROUP
};
/// Base class for variables.
///
/// Provides naming and lookup services for variables.
///
class AP_Param
{
public:
// the Info and GroupInfo structures are passed by the main
// program in setup() to give information on how variables are
// named and their location in memory
struct GroupInfo {
uint8_t type; // AP_PARAM_*
uint8_t idx; // identifier within the group
const char name[AP_MAX_NAME_SIZE];
uintptr_t offset; // offset within the object
const struct GroupInfo *group_info;
};
struct Info {
uint8_t type; // AP_PARAM_*
const char name[AP_MAX_NAME_SIZE];
uint8_t key; // k_param_*
void *ptr; // pointer to the variable in memory
const struct GroupInfo *group_info;
};
// called once at startup to setup the _var_info[] table. This
// will also check the EEPROM header and re-initialise it if the
// wrong version is found
static bool setup(const struct Info *info, uint8_t num_vars, uint16_t eeprom_size);
/// Copy the variable's name, prefixed by any containing group name, to a buffer.
///
/// If the variable has no name, the buffer will contain an empty string.
///
/// Note that if the combination of names is larger than the buffer, the
/// result in the buffer will be truncated.
///
/// @param buffer The destination buffer
/// @param bufferSize Total size of the destination buffer.
///
void copy_name(char *buffer, size_t bufferSize);
/// Find a variable by name.
///
/// If the variable has no name, it cannot be found by this interface.
///
/// @param name The full name of the variable to be found.
/// @return A pointer to the variable, or NULL if
/// it does not exist.
///
static AP_Param *find(const char *name, enum ap_var_type *ptype);
/// Save the current value of the variable to EEPROM.
///
/// @return True if the variable was saved successfully.
///
bool save(void);
/// Load the variable from EEPROM.
///
/// @return True if the variable was loaded successfully.
///
bool load(void);
/// Load all variables from EEPROM
///
/// This function performs a best-efforts attempt to load all
/// of the variables from EEPROM. If some fail to load, their
/// values will remain as they are.
///
/// @return False if any variable failed to load
///
static bool load_all(void);
/// Erase all variables in EEPROM.
///
static void erase_all(void);
/// print the value of all variables
static void show_all(void);
/// Returns the first variable
///
/// @return The first variable in _var_info, or NULL if
/// there are none.
///
static AP_Param *first(uint16_t *token, enum ap_var_type *ptype);
/// Returns the next variable in _var_info, recursing into groups
/// as needed
static AP_Param *next(uint16_t *token, enum ap_var_type *ptype);
/// Returns the next scalar variable in _var_info, recursing into groups
/// as needed
static AP_Param *next_scalar(uint16_t *token, enum ap_var_type *ptype);
/// cast a variable to a float given its type
float cast_to_float(enum ap_var_type type);
private:
/// EEPROM header
///
/// This structure is placed at the head of the EEPROM to indicate
/// that the ROM is formatted for AP_Param.
///
struct EEPROM_header {
uint8_t magic[2];
uint8_t revision;
uint8_t spare;
};
// This header is prepended to a variable stored in EEPROM.
struct Param_header {
uint8_t key;
uint8_t group_element;
uint8_t type;
};
// number of bits in each level of nesting of groups
static const uint8_t _group_level_shift = 4;
static const uint8_t _group_bits = 8;
static const uint8_t _sentinal_key = 0xFF;
static const uint8_t _sentinal_type = 0xFF;
static const uint8_t _sentinal_group = 0xFF;
static bool check_group_info(const struct GroupInfo *group_info, uint16_t *total_size, uint8_t max_bits);
static bool check_var_info(void);
const struct Info *find_var_info_group(const struct GroupInfo *group_info,
uint8_t vindex,
uint8_t group_base,
uint8_t group_shift,
uint8_t *group_element,
const struct GroupInfo **group_ret);
const struct Info *find_var_info(uint8_t *group_element,
const struct GroupInfo **group_ret);
static const struct Info *find_by_header_group(struct Param_header phdr, void **ptr,
uint8_t vindex,
const struct GroupInfo *group_info,
uint8_t group_base,
uint8_t group_shift);
static const struct Info *find_by_header(struct Param_header phdr, void **ptr);
static AP_Param *find_group(const char *name, uint8_t vindex, const struct GroupInfo *group_info, enum ap_var_type *ptype);
static void write_sentinal(uint16_t ofs);
bool scan(const struct Param_header *phdr, uint16_t *pofs);
static const uint8_t type_size(enum ap_var_type type);
static void eeprom_write_check(const void *ptr, uint16_t ofs, uint8_t size);
static AP_Param *next_group(uint8_t vindex, const struct GroupInfo *group_info,
bool *found_current,
uint8_t group_base,
uint8_t group_shift,
uint16_t *token,
enum ap_var_type *ptype);
static uint16_t _eeprom_size;
static uint8_t _num_vars;
static const struct Info *_var_info;
// values filled into the EEPROM header
static const uint8_t k_EEPROM_magic0 = 0x50;
static const uint8_t k_EEPROM_magic1 = 0x41; ///< "AP"
static const uint8_t k_EEPROM_revision = 5; ///< current format revision
};
/// Template class for scalar variables.
///
/// Objects of this type have a value, and can be treated in many ways as though they
/// were the value.
///
/// @tparam T The scalar type of the variable
/// @tparam PT The AP_PARAM_* type
///
template<typename T, ap_var_type PT>
class AP_ParamT : public AP_Param
{
public:
/// Constructor for scalar variable.
///
/// Initialises a stand-alone variable with optional initial value.
///
/// @param default_value Value the variable should have at startup.
///
AP_ParamT<T,PT> (const T initial_value = 0) :
AP_Param(),
_value(initial_value)
{
}
static const ap_var_type vtype = PT;
/// Value getter
///
T get(void) const {
return _value;
}
/// Value setter
///
void set(T v) {
_value = v;
}
/// Combined set and save
///
bool set_and_save(T v) {
set(v);
return save();
}
/// Conversion to T returns a reference to the value.
///
/// This allows the class to be used in many situations where the value would be legal.
///
operator T &() {
return _value;
}
/// Copy assignment from self does nothing.
///
AP_ParamT<T,PT>& operator=(AP_ParamT<T,PT>& v) {
return v;
}
/// Copy assignment from T is equivalent to ::set.
///
AP_ParamT<T,PT>& operator=(T v) {
_value = v;
return *this;
}
/// AP_ParamT types can implement AP_Param::cast_to_float
///
float cast_to_float(void) {
return (float)_value;
}
protected:
T _value;
};
/// Template class for non-scalar variables.
///
/// Objects of this type have a value, and can be treated in many ways as though they
/// were the value.
///
/// @tparam T The scalar type of the variable
/// @tparam PT AP_PARAM_* type
///
template<typename T, ap_var_type PT>
class AP_ParamV : public AP_Param
{
public:
static const ap_var_type vtype = PT;
/// Value getter
///
T get(void) const {
return _value;
}
/// Value setter
///
void set(T v) {
_value = v;
}
/// Combined set and save
///
bool set_and_save(T v) {
set(v);
return save();
}
/// Conversion to T returns a reference to the value.
///
/// This allows the class to be used in many situations where the value would be legal.
///
operator T &() {
return _value;
}
/// Copy assignment from self does nothing.
///
AP_ParamT<T,PT>& operator=(AP_ParamT<T,PT>& v) {
return v;
}
/// Copy assignment from T is equivalent to ::set.
///
AP_ParamT<T,PT>& operator=(T v) {
_value = v;
return *this;
}
protected:
T _value;
};
/// Template class for array variables.
///
/// Objects created using this template behave like arrays of the type T,
/// but are stored like single variables.
///
/// @tparam T The scalar type of the variable
/// @tparam N number of elements
/// @tparam PT the AP_PARAM_* type
///
template<typename T, uint8_t N, ap_var_type PT>
class AP_ParamA : public AP_Param
{
public:
static const ap_var_type vtype = PT;
/// Array operator accesses members.
///
/// @note It would be nice to range-check i here, but then what would we return?
///
T &operator [](uint8_t i) {
return _value[i];
}
/// Value getter
///
/// @note Returns zero for index values out of range.
///
T get(uint8_t i) const {
if (i < N) {
return _value[i];
} else {
return (T)0;
}
}
/// Value setter
///
/// @note Attempts to set an index out of range are discarded.
///
void set(uint8_t i, T v) {
if (i < N) {
_value[i] = v;
}
}
/// Copy assignment from self does nothing.
///
AP_ParamA<T,N,PT>& operator=(AP_ParamA<T,N,PT>& v) {
return v;
}
protected:
T _value[N];
};
/// Convenience macro for defining instances of the AP_ParamT template.
///
#define AP_PARAMDEF(_t, _n, _pt) typedef AP_ParamT<_t, _pt> AP_##_n;
AP_PARAMDEF(float, Float, AP_PARAM_FLOAT); // defines AP_Float
AP_PARAMDEF(int8_t, Int8, AP_PARAM_INT8); // defines AP_Int8
AP_PARAMDEF(int16_t, Int16, AP_PARAM_INT16); // defines AP_Int16
AP_PARAMDEF(int32_t, Int32, AP_PARAM_INT32); // defines AP_Int32
#define AP_PARAMDEFV(_t, _n, _pt) typedef AP_ParamV<_t, _pt> AP_##_n;
AP_PARAMDEFV(Matrix3f, Matrix3f, AP_PARAM_MATRIX3F);
AP_PARAMDEFV(Vector3f, Vector3f, AP_PARAM_VECTOR3F);
#define AP_PARAMDEFA(_t, _n, _size, _pt) typedef AP_ParamA<_t, _size, _pt> AP_##_n;
AP_PARAMDEFA(float, Vector6f, 6, AP_PARAM_VECTOR6F);
/// Rely on built in casting for other variable types
/// to minimize template creation and save memory
#define AP_Uint8 AP_Int8
#define AP_Uint16 AP_Int16
#define AP_Uint32 AP_Int32
#define AP_Bool AP_Int8
#endif // AP_PARAM_H

130
libraries/AP_Common/AP_Var_menufuncs.cpp
View File

@ -13,133 +13,3 @@
#include <FastSerial.h>
#include <AP_Common.h>
void
AP_Var_print(AP_Var *vp)
{
// try to print from variable types that we know
if (vp->meta_type_id() == AP_Var::k_typeid_float) {
AP_Float *v = (AP_Float *)vp;
Serial.printf_P(PSTR("%f"), v->get());
} else if (vp->meta_type_id() == AP_Var::k_typeid_float16) {
AP_Float16 *v = (AP_Float16 *)vp;
Serial.printf_P(PSTR("%f"), v->get());
} else if (vp->meta_type_id() == AP_Var::k_typeid_int32) {
AP_Int32 *v = (AP_Int32 *)vp;
Serial.printf_P(PSTR("%ld"), v->get());
} else if (vp->meta_type_id() == AP_Var::k_typeid_int16) {
AP_Int16 *v = (AP_Int16 *)vp;
Serial.printf_P(PSTR("%d"), v->get());
} else if (vp->meta_type_id() == AP_Var::k_typeid_int8) {
AP_Int8 *v = (AP_Int8 *)vp;
Serial.printf_P(PSTR("%d"), v->get());
} else {
Serial.print_P(PSTR("??"));
}
}
int8_t
AP_Var_menu_set(uint8_t argc, const Menu::arg *argv)
{
AP_Var *vp;
// check argument count
if (argc != 3) {
Serial.println_P(PSTR("missing name or value"));
return -1;
}
Serial.printf_P(PSTR("%s: "), argv[1].str);
// search for the variable
vp = AP_Var::find(argv[1].str);
if (NULL == vp) {
Serial.println_P(PSTR("not found"));
return -1;
}
// try to assign to variable types that we know
if (vp->meta_type_id() == AP_Var::k_typeid_float) {
AP_Float *v = (AP_Float *)vp;
v->set(argv[2].f);
} else if (vp->meta_type_id() == AP_Var::k_typeid_float16) {
AP_Float16 *v = (AP_Float16 *)vp;
v->set(argv[2].f);
} else if (vp->meta_type_id() == AP_Var::k_typeid_int32) {
AP_Int32 *v = (AP_Int32 *)vp;
v->set(argv[2].i);
} else if (vp->meta_type_id() == AP_Var::k_typeid_int16) {
AP_Int16 *v = (AP_Int16 *)vp;
v->set(argv[2].i);
} else if (vp->meta_type_id() == AP_Var::k_typeid_int8) {
AP_Int8 *v = (AP_Int8 *)vp;
v->set(argv[2].i);
} else {
Serial.println_P(PSTR("unknown type"));
return -1;
}
AP_Var_print(vp);
Serial.println();
return 0;
}
int8_t
AP_Var_menu_show(uint8_t argc, const Menu::arg *argv)
{
AP_Var *vp;
// if no arguments, show all variables
if (argc == 1) {
for (vp = AP_Var::first(); NULL != vp; vp = vp->next()) {
char name_buffer[32];
// groups should not be displayed
if (vp->meta_type_id() == AP_Var::k_typeid_group) {
continue;
}
// get a displayable name for the variable
vp->copy_name(name_buffer, sizeof(name_buffer));
if (name_buffer[0] == 0) {
// without a name the variable is not displayable
continue;
}
// print name and value
Serial.printf_P(PSTR("%03u:%-32.32s: "), vp->key(), name_buffer);
AP_Var_print(vp);
Serial.println();
}
return 0;
}
// show variable by name
vp = AP_Var::find(argv[1].str);
if (NULL == vp) {
Serial.println_P(PSTR("not found"));
return -1;
}
Serial.printf_P(PSTR("%03u:%s: "), vp->key(), argv[1].str);
AP_Var_print(vp);
Serial.println();
return 0;
}

2
libraries/AP_Common/examples/AP_Var/Makefile
View File

@ -1,2 +0,0 @@
BOARD = mega
include ../../Arduino.mk

137
libraries/AP_Common/tools/eedump_apparam.c Normal file
View File

@ -0,0 +1,137 @@
/*
* Simple tool to dump the AP_Param contents from an EEPROM dump
* Andrew Tridgell February 2012
*/
#include <stdio.h>
#include <stdlib.h>
#include <stdint.h>
uint8_t eeprom[0x1000];
#pragma pack(1)
struct EEPROM_header {
uint8_t magic[2];
uint8_t revision;
uint8_t spare;
};
static const uint16_t k_EEPROM_magic0 = 0x50;
static const uint16_t k_EEPROM_magic1 = 0x41;
static const uint16_t k_EEPROM_revision = 5;
enum ap_var_type {
AP_PARAM_NONE = 0,
AP_PARAM_INT8,
AP_PARAM_INT16,
AP_PARAM_INT32,
AP_PARAM_FLOAT,
AP_PARAM_VECTOR3F,
AP_PARAM_VECTOR6F,
AP_PARAM_MATRIX3F,
AP_PARAM_GROUP
};
static const char *type_names[8] = {
"NONE", "INT8", "INT16", "INT32", "FLOAT", "VECTOR3F", "MATRIX6F", "GROUP"
};
struct Param_header {
uint8_t key;
uint8_t group_element;
uint8_t type;
};
static const uint8_t _sentinal_key = 0xFF;
static const uint8_t _sentinal_type = 0xFF;
static const uint8_t _sentinal_group = 0xFF;
static uint8_t type_size(enum ap_var_type type)
{
switch (type) {
case AP_PARAM_NONE:
case AP_PARAM_GROUP:
return 0;
case AP_PARAM_INT8:
return 1;
case AP_PARAM_INT16:
return 2;
case AP_PARAM_INT32:
return 4;
case AP_PARAM_FLOAT:
return 4;
case AP_PARAM_VECTOR3F:
return 3*4;
case AP_PARAM_VECTOR6F:
return 6*4;
case AP_PARAM_MATRIX3F:
return 3*3*4;
}
printf("unknown type %u\n", type);
return 0;
}
static void
fail(const char *why)
{
fprintf(stderr, "ERROR: %s\n", why);
exit(1);
}
int
main(int argc, char *argv[])
{
FILE *fp;
struct EEPROM_header *header;
struct Param_header *var;
unsigned index;
unsigned i;
if (argc != 2) {
fail("missing EEPROM file name");
}
if (NULL == (fp = fopen(argv[1], "rb"))) {
fail("can't open EEPROM file");
}
if (1 != fread(eeprom, sizeof(eeprom), 1, fp)) {
fail("can't read EEPROM file");
}
fclose(fp);
header = (struct EEPROM_header *)&eeprom[0];
if (header->magic[0] != k_EEPROM_magic0 ||
header->magic[1] != k_EEPROM_magic1) {
fail("bad magic in EEPROM file");
}
if (header->revision != k_EEPROM_revision) {
fail("unsupported EEPROM format revision");
}
printf("Header OK\n");
index = sizeof(*header);
for (;;) {
uint8_t size;
var = (struct Param_header *)&eeprom[index];
if (var->key == _sentinal_key ||
var->group_element == _sentinal_group ||
var->type == _sentinal_type) {
printf("end sentinel at %u\n", index);
break;
}
size = type_size(var->type);
printf("%04x: type %u (%s) key %u group_element %u size %d\n ",
index, var->type, type_names[var->type], var->key, var->group_element, size);
index += sizeof(*var);
for (i = 0; i < size; i++) {
printf(" %02x", eeprom[index + i]);
}
printf("\n");
index += size;
if (index >= sizeof(eeprom)) {
fflush(stdout);
fail("missing end sentinel");
}
}
return 0;
}

78
libraries/AP_Common/tools/eedump_apparam.pl Normal file
View File

@ -0,0 +1,78 @@
#!/usr/bin/perl
$file = $ARGV[0];
open(IN,$file) || die print "Failed to open file: $file : $!";
read(IN,$buffer,1);
read(IN,$buffer2,1);
if (ord($buffer2) != 0x41 && ord($buffer) != 0x50) {
print "bad header ". $buffer ." ".$buffer2. "\n";
exit;
}
read(IN,$buffer,1);
if (ord($buffer) != 5) {
print "bad version";
exit;
}
# spare
read(IN,$buffer,1);
$a = 0;
while (read(IN,$buffer,1)) {
$pos = (tell(IN) - 1);
if (ord($buffer) == 0xff) {
printf("end sentinel at %u\n", $pos);
last;
}
read(IN,$buffer2,1);
read(IN,$buffer3,1);
if (ord($buffer3) == 0) { #none
$size = 0;
$type = "NONE";
} elsif (ord($buffer3) == 1) { #int8
$size = 1;
$type = "INT8";
} elsif (ord($buffer3) == 2) { #int16
$size = 2;
$type = "INT16";
} elsif (ord($buffer3) == 3) { #int32
$size = 4;
$type = "INT32";
} elsif (ord($buffer3) == 4) { #float
$size = 4;
$type = "FLOAT";
} elsif (ord($buffer3) == 5) { #vector 3
$size = 3*4;
$type = "VECTOR3F";
} elsif (ord($buffer3) == 6) { #vector6
$size = 6*4;
$type = "VECTOR6F";
} elsif (ord($buffer3) == 7) { #matrix
$size = 3*3*4;
$type = "MATRIX6F";
} elsif (ord($buffer3) == 8) { #group
$size = 0;
$type = "GROUP";
} else {
print "Unknown type\n";
$size = 0;
}
printf("%04x: type %u ($type) key %u group_element %u size %d\n ", $pos, ord($buffer3),ord($buffer),ord($buffer2), $size);
for ($i = 0; $i < ($size); $i++) {
read(IN,$buffer,1);
printf(" %02x", ord($buffer));
}
print "\n";
}
close IN;

10
libraries/AP_Compass/AP_Compass_HIL.cpp
View File

@ -23,9 +23,9 @@ bool AP_Compass_HIL::read()
//
void AP_Compass_HIL::setHIL(float _mag_x, float _mag_y, float _mag_z)
{
// TODO: map floats to raw
mag_x = _mag_x;
mag_y = _mag_y;
mag_z = _mag_z;
healthy = true;
Vector3f ofs = _offset.get();
mag_x = _mag_x + ofs.x;
mag_y = _mag_y + ofs.y;
mag_z = _mag_z + ofs.z;
healthy = true;
}

2
libraries/AP_Compass/AP_Compass_HIL.h
View File

@ -7,7 +7,7 @@
class AP_Compass_HIL : public Compass
{
public:
AP_Compass_HIL(AP_Var::Key key = AP_Var::k_key_none) : Compass(key) { product_id = AP_COMPASS_TYPE_HIL; }
AP_Compass_HIL() : Compass() { product_id = AP_COMPASS_TYPE_HIL; }
bool read(void);
void setHIL(float Mag_X, float Mag_Y, float Mag_Z);
};

2
libraries/AP_Compass/AP_Compass_HMC5843.cpp
View File

@ -75,7 +75,7 @@ bool AP_Compass_HMC5843::write_register(uint8_t address, byte value)
the 5883L has a different orientation to the 5843. This allows us to
use a single MAG_ORIENTATION for both
*/
static void rotate_for_5883L(AP_VarS<Matrix3f> *_orientation_matrix)
static void rotate_for_5883L(AP_Matrix3f *_orientation_matrix)
{
_orientation_matrix->set_and_save(_orientation_matrix->get() * Matrix3f(ROTATION_YAW_90));
}

2
libraries/AP_Compass/AP_Compass_HMC5843.h
View File

@ -56,7 +56,7 @@ class AP_Compass_HMC5843 : public Compass
uint32_t _retry_time; // when unhealthy the millis() value to retry at
public:
AP_Compass_HMC5843(AP_Var::Key key = AP_Var::k_key_none) : Compass(key) {}
AP_Compass_HMC5843() : Compass() {}
virtual bool init(void);
virtual bool read(void);
virtual void set_orientation(const Matrix3f &rotation_matrix);

18
libraries/AP_Compass/Compass.cpp
View File

@ -1,15 +1,19 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#include "Compass.h"
const AP_Param::GroupInfo Compass::var_info[] PROGMEM = {
AP_GROUPINFO("ORIENT", 0, Compass, _orientation_matrix),
AP_GROUPINFO("OFS", 1, Compass, _offset),
AP_GROUPINFO("DEC", 2, Compass, _declination),
AP_GROUPEND
};
// Default constructor.
// Note that the Vector/Matrix constructors already implicitly zero
// their values.
//
Compass::Compass(AP_Var::Key key) :
_group(key, PSTR("COMPASS_")),
_orientation_matrix (&_group, 0),
_offset (&_group, 1),
_declination (&_group, 2, 0.0, PSTR("DEC")),
Compass::Compass(void) :
_declination (0.0),
_null_init_done(false),
_null_enable(false),
product_id(AP_COMPASS_TYPE_UNKNOWN)
@ -50,7 +54,7 @@ Compass::save_offsets()
Vector3f &
Compass::get_offsets()
{
return _offset.get();
return _offset;
}
void
@ -183,4 +187,4 @@ Compass::null_offsets_disable(void)
{
_null_init_done = false;
_null_enable = false;
}
}

19
libraries/AP_Compass/Compass.h
View File

@ -33,21 +33,21 @@
class Compass
{
public:
int product_id; /// product id
int mag_x; ///< magnetic field strength along the X axis
int mag_y; ///< magnetic field strength along the Y axis
int mag_z; ///< magnetic field strength along the Z axis
int16_t product_id; /// product id
int16_t mag_x; ///< magnetic field strength along the X axis
int16_t mag_y; ///< magnetic field strength along the Y axis
int16_t mag_z; ///< magnetic field strength along the Z axis
float heading; ///< compass heading in radians
float heading_x; ///< compass vector X magnitude
float heading_y; ///< compass vector Y magnitude
unsigned long last_update; ///< millis() time of last update
uint32_t last_update; ///< millis() time of last update
bool healthy; ///< true if last read OK
/// Constructor
///
/// @param key Storage key used for configuration data.
///
Compass(AP_Var::Key key);
Compass();
/// Initialize the compass device.
///
@ -132,10 +132,11 @@ public:
virtual void set_declination(float radians);
float get_declination();
static const struct AP_Param::GroupInfo var_info[];
protected:
AP_Var_group _group; ///< storage group holding the compass' calibration data
AP_VarS<Matrix3f> _orientation_matrix;
AP_VarS<Vector3f> _offset;
AP_Matrix3f _orientation_matrix;
AP_Vector3f _offset;
AP_Float _declination;
bool _null_enable; ///< enabled flag for offset nulling

2
libraries/AP_DCM/AP_DCM.cpp
View File

@ -268,6 +268,8 @@ AP_DCM::renorm(Vector3f const &a, int &problem)
renorm_sqrt_count++;
} else {
problem = 1;
SITL_debug("ERROR: DCM renormalisation error. renorm_val=%f\n",
renorm_val);
renorm_blowup_count++;
}

6
libraries/AP_GPS/AP_GPS_Auto.cpp
View File

@ -97,9 +97,9 @@ GPS *
AP_GPS_Auto::_detect(void)
{
unsigned long then;
int fingerprint[4];
int tries;
int charcount;
uint8_t fingerprint[4];
uint8_t tries;
uint16_t charcount;
GPS *gps;
//

4
libraries/AP_GPS/AP_GPS_MTK.cpp
View File

@ -28,8 +28,8 @@ AP_GPS_MTK::init(void)
// XXX should assume binary, let GPS_AUTO handle dynamic config?
_port->print(MTK_SET_BINARY);
// set 10Hz update rate
_port->print(MTK_OUTPUT_10HZ);
// set 4Hz update rate
_port->print(MTK_OUTPUT_4HZ);
// set initial epoch code
_epoch = TIME_OF_DAY;

4
libraries/AP_GPS/AP_GPS_MTK16.cpp
View File

@ -34,8 +34,8 @@ AP_GPS_MTK16::init(void)
// XXX should assume binary, let GPS_AUTO handle dynamic config?
_port->print(MTK_SET_BINARY);
// set 10Hz update rate
_port->print(MTK_OUTPUT_10HZ);
// set 4Hz update rate
_port->print(MTK_OUTPUT_4HZ);
// set initial epoch code
_epoch = TIME_OF_DAY;

5
libraries/AP_IMU/AP_IMU_INS.cpp
View File

@ -21,6 +21,11 @@
#include "AP_IMU_INS.h"
const AP_Param::GroupInfo AP_IMU_INS::var_info[] PROGMEM = {
AP_GROUPINFO("CAL", 0, AP_IMU_INS, _sensor_cal),
AP_GROUPEND
};
void
AP_IMU_INS::init( Start_style style,
void (*delay_cb)(unsigned long t),

8
libraries/AP_IMU/AP_IMU_INS.h
View File

@ -26,9 +26,8 @@ public:
/// @param adc Pointer to the AP_ADC instance that is connected to the gyro and accelerometer.
/// @param key The AP_Var::key value we will use when loading/saving calibration data.
///
AP_IMU_INS(AP_InertialSensor *ins, AP_Var::Key key) :
_ins(ins),
_sensor_cal(key, PSTR("IMU_SENSOR_CAL"))
AP_IMU_INS(AP_InertialSensor *ins) :
_ins(ins)
{}
/// Do warm or cold start.
@ -65,10 +64,11 @@ public:
virtual void ay(const float v) { _sensor_cal[4] = v; }
virtual void az(const float v) { _sensor_cal[5] = v; }
static const struct AP_Param::GroupInfo var_info[];
private:
AP_InertialSensor *_ins; ///< INS provides an axis and unit correct sensor source.
AP_VarA<float,6> _sensor_cal; ///< Calibrated sensor offsets
AP_Vector6f _sensor_cal; ///< Calibrated sensor offsets
virtual void _init_accel(void (*delay_cb)(unsigned long t),
void (*flash_leds_cb)(bool on) = NULL); ///< no-save implementation

3
libraries/AP_Common/AP_Var.cpp → libraries/AP_Var/AP_Var.cpp
View File

@ -9,9 +9,10 @@
/// @file AP_Var.cpp
/// @brief The AP variable store.
#define USE_AP_VAR
#include <AP_Common.h>
#include <AP_Var.h>
#include <math.h>
#include <string.h>

2
libraries/AP_Common/AP_Var.h → libraries/AP_Var/AP_Var.h
View File

@ -943,7 +943,7 @@ extern AP_Float AP_Float_zero;
extern void AP_Var_print(AP_Var *vp);
#ifndef __AP_COMMON_MENU_H
# error Must include menu.h
#include "include/menu.h"
#endif
/// Menu function for setting an AP_Var.

3
libraries/AP_Common/examples/AP_Var/AP_Var.pde → libraries/AP_Var/examples/AP_Var/AP_Var.pde
View File

@ -2,9 +2,12 @@
// Unit tests for the AP_Meta_class and AP_Var classes.
//
#define USE_AP_VAR
#include <FastSerial.h>
#include <AP_Common.h>
#include <AP_Test.h>
#include <AP_Var.h>
#include <AP_Math.h>
#include <string.h>
// we need to do this, even though normally it's a bad idea

1
libraries/AP_Var/examples/AP_Var/Makefile Normal file
View File

@ -0,0 +1 @@
include ../../../AP_Common/Arduino.mk

1
libraries/Desktop/include/avr/pgmspace.h
View File

@ -27,6 +27,7 @@ extern "C" {
typedef char PROGMEM prog_char;
extern int strcasecmp_P(const char *, PGM_P) __ATTR_PURE__;
extern int strcmp_P(const char *, PGM_P) __ATTR_PURE__;
extern int strncmp_P(const char *, PGM_P, size_t n) __ATTR_PURE__;
extern size_t strlcat_P (char *, PGM_P, size_t );
extern size_t strnlen_P (PGM_P, size_t );
extern size_t strlen_P (PGM_P);

10
libraries/Desktop/support/Arduino.cpp
View File

@ -87,6 +87,16 @@ int strcmp_P(PGM_P str1, PGM_P str2)
return strcmp(str1, str2);
}
int strncmp_P(PGM_P str1, PGM_P str2, size_t n)
{
return strncmp(str1, str2, n);
}
char *strncpy_P(char *dest, PGM_P src, size_t n)
{
return strncpy(dest, src, n);
}
void *memcpy_P(void *dest, PGM_P src, size_t n)
{
return memcpy(dest, src, n);

9
libraries/Desktop/support/main.cpp
View File

@ -15,6 +15,13 @@ void loop(void);
// the state of the desktop simulation
struct desktop_info desktop_state;
// catch floating point exceptions
static void sig_fpe(int signum)
{
printf("ERROR: Floating point exception\n");
exit(1);
}
static void usage(void)
{
printf("Options:\n");
@ -31,6 +38,8 @@ int main(int argc, char * const argv[])
desktop_state.slider = false;
gettimeofday(&desktop_state.sketch_start_time, NULL);
signal(SIGFPE, sig_fpe);
while ((opt = getopt(argc, argv, "swhr:H:")) != -1) {
switch (opt) {
case 's':

8
libraries/Desktop/support/sitl_compass.cpp
View File

@ -16,6 +16,10 @@
#include "desktop.h"
#include "util.h"
#define MAG_OFS_X 17.0
#define MAG_OFS_Y 125.0
#define MAG_OFS_Z -18.0
/*
given a magnetic heading, and roll, pitch, yaw values,
calculate consistent magnetometer components
@ -44,7 +48,7 @@ static Vector3f heading_to_mag(float heading, float roll, float pitch, float yaw
cos_pitch = 1.0e-10;
}
v.z = -0.4;
v.z = -0.6*cos(roll)*cos(pitch);
v.y = (headY + v.z*sin_roll) / cos_roll;
v.x = (headX - (v.y*sin_roll*sin_pitch + v.z*cos_roll*sin_pitch)) / cos_pitch;
scale = magnitude / sqrt((v.x*v.x) + (v.y*v.y) + (v.z*v.z));
@ -65,5 +69,5 @@ void sitl_update_compass(float heading, float roll, float pitch, float yaw)
ToRad(roll),
ToRad(pitch),
ToRad(yaw));
compass.setHIL(m.x, m.y, m.z);
compass.setHIL(m.x - MAG_OFS_X, m.y - MAG_OFS_Y, m.z - MAG_OFS_Z);
}

3
libraries/Desktop/support/sitl_gps.cpp
View File

@ -146,6 +146,9 @@ void sitl_update_gps(double latitude, double longitude, float altitude,
velned.speed_3d = velned.speed_2d;
lon_scale = cos(ToRad(latitude));
velned.heading_2d = ToDeg(atan2(lon_scale*speedE, speedN)) * 100000.0;
if (velned.heading_2d < 0.0) {
velned.heading_2d += 360.0 * 100000.0;
}
velned.speed_accuracy = 2;
velned.heading_accuracy = 4;

10
libraries/GCS_MAVLink/Mavlink_compat.h
View File

@ -53,15 +53,15 @@
#define mavlink_msg_waypoint_set_current_decode mavlink_msg_mission_set_current_decode
#define mavlink_waypoint_set_current_t mavlink_mission_set_current_t
static uint8_t mav_var_type(AP_Meta_class::Type_id t)
static uint8_t mav_var_type(enum ap_var_type t)
{
if (t == AP_Var::k_typeid_int8) {
if (t == AP_PARAM_INT8) {
return MAV_VAR_INT8;
}
if (t == AP_Var::k_typeid_int16) {
if (t == AP_PARAM_INT16) {
return MAV_VAR_INT16;
}
if (t == AP_Var::k_typeid_int32) {
if (t == AP_PARAM_INT32) {
return MAV_VAR_INT32;
}
// treat any others as float
@ -72,7 +72,7 @@ static uint8_t mav_var_type(AP_Meta_class::Type_id t)
#else // MAVLINK10
static uint8_t mav_var_type(AP_Meta_class::Type_id t)
static uint8_t mav_var_type(enum ap_var_type t)
{
return 0;
}

18
libraries/PID/PID.cpp
View File

@ -7,6 +7,14 @@
#include "PID.h"
const AP_Param::GroupInfo PID::var_info[] PROGMEM = {
AP_GROUPINFO("P", 0, PID, _kp),
AP_GROUPINFO("I", 1, PID, _ki),
AP_GROUPINFO("D", 2, PID, _kd),
AP_GROUPINFO("IMAX", 3, PID, _imax),
AP_GROUPEND
};
long
PID::get_pid(int32_t error, uint16_t dt, float scaler)
{
@ -62,11 +70,17 @@ PID::reset_I()
void
PID::load_gains()
{
_group.load();
_kp.load();
_ki.load();
_kd.load();
_imax.load();
}
void
PID::save_gains()
{
_group.save();
_kp.save();
_ki.save();
_kd.save();
_imax.save();
}

Some files were not shown because too many files have changed in this diff Show More