aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Copter: merge the latest 2.9 changes into master

Browse Source
This commit is contained in:
Andrew Tridgell 2013-01-08 16:45:12 +11:00 committed by geermc4
parent c9fe7fe932
commit 860f4b2605
4 changed files with 287 additions and 201 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

257
ArduCopter/ArduCopter.pde
View File

@ -375,12 +375,21 @@ static struct AP_System{
uint8_t nav_ok : 1; // 4 // deprecated
uint8_t CH7_flag : 1; // 5 // manages state of the ch7 toggle switch
uint8_t usb_connected : 1; // 6 // true if APM is powered from USB connection
uint8_t run_50hz_loop : 1; // 7 // toggles the 100hz loop for 50hz
uint8_t alt_sensor_flag : 1; // 8 // used to track when to read sensors vs estimate alt
uint8_t yaw_stopped : 1; // 9 // Used to manage the Yaw hold capabilities
uint8_t alt_sensor_flag : 1; // 7 // used to track when to read sensors vs estimate alt
uint8_t yaw_stopped : 1; // 8 // Used to manage the Yaw hold capabilities
} ap_system;
/*
what navigation updated are needed
*/
static struct nav_updates {
uint8_t need_velpos : 1;
uint8_t need_dist_bearing : 1;
uint8_t need_nav_controllers : 1;
uint8_t need_nav_pitch_roll : 1;
} nav_updates;
////////////////////////////////////////////////////////////////////////////////
// velocity in lon and lat directions calculated from GPS position and accelerometer data
@ -448,6 +457,8 @@ MOTOR_CLASS motors(&g.rc_1, &g.rc_2, &g.rc_3, &g.rc_4);
static Vector3f omega;
// This is used to hold radio tuning values for in-flight CH6 tuning
float tuning_value;
// used to limit the rate that the pid controller output is logged so that it doesn't negatively affect performance
static uint8_t pid_log_counter;
////////////////////////////////////////////////////////////////////////////////
// LED output
@ -814,14 +825,10 @@ static int16_t gps_fix_count;
// --------------
// Time in microseconds of main control loop
static uint32_t fast_loopTimer;
// Time in microseconds of 50hz control loop
static uint32_t fiftyhz_loopTimer;
// Counters for branching from 10 hz control loop
static uint8_t medium_loopCounter;
// Counters for branching from 3 1/3hz control loop
static uint8_t slow_loopCounter;
// Counters for branching at 1 hz
static uint8_t counter_one_herz;
// Counter of main loop executions. Used for performance monitoring and failsafe processing
static uint16_t mainLoop_count;
// Delta Time in milliseconds for navigation computations, updated with every good GPS read
@ -928,15 +935,130 @@ void setup() {
}
/*
return true if the main loop is ready to run. This is used by
potentially expensive functions that are not timing critical, to
defer the expensive processing until after the main loop has run.
if the compass is enabled then try to accumulate a reading
*/
static bool main_loop_ready(void)
static void compass_accumulate(void)
{
return ins.num_samples_available() >= 2;
if (g.compass_enabled) {
compass.accumulate();
}
}
// enable this to get console logging of scheduler performance
#define SCHEDULER_DEBUG 0
static void perf_update(void)
{
if (g.log_bitmask & MASK_LOG_PM)
Log_Write_Performance();
#if SCHEDULER_DEBUG
cliSerial->printf_P(PSTR("PERF: %u/%u %lu\n"),
(unsigned)perf_info_get_num_long_running(),
(unsigned)perf_info_get_num_loops(),
(unsigned long)perf_info_get_max_time());
#endif
perf_info_reset();
gps_fix_count = 0;
}
/*
return number of micros until the main loop will want to run again
*/
static int16_t main_loop_time_available(void)
{
uint32_t dt = (micros() - fast_loopTimer);
if (dt > 10000) {
return 0;
}
return 10000 - dt;
}
typedef void (*event_fn_t)(void);
struct timer_event_table {
event_fn_t func;
uint16_t time_interval_10ms;
uint16_t min_time_usec;
};
#define NUM_TIMER_EVENTS 14
/*
scheduler table - all regular events apart from the fast_loop()
should be listed here, along with how often they should be called
(in 10ms units) and the maximum time they are expected to take
*/
static const struct timer_event_table PROGMEM timer_events[NUM_TIMER_EVENTS] = {
{ update_GPS, 2, 900 },
{ update_navigation, 2, 500 },
{ medium_loop, 2, 700 },
{ update_altitude_est, 2, 900 },
{ fifty_hz_loop, 2, 800 },
{ run_nav_updates, 2, 500 },
{ slow_loop, 10, 500 },
{ gcs_check_input, 2, 700 },
{ gcs_send_heartbeat, 100, 700 },
{ gcs_data_stream_send, 2, 1100 },
{ gcs_send_deferred, 2, 700 },
{ compass_accumulate, 2, 600 },
{ super_slow_loop, 100, 1100 },
{ perf_update, 1000, 500 }
};
static uint16_t timer_counters[NUM_TIMER_EVENTS];
static uint16_t tick_counter;
static uint32_t event_time_started;
static uint16_t event_time_allowed;
/*
return number of micros until the current event reaches its deadline
*/
static uint16_t event_time_available(void)
{
uint32_t dt = micros() - event_time_started;
if (dt > event_time_allowed) {
return 0;
}
return event_time_allowed - dt;
}
/*
run as many scheduler events as we can
*/
static void run_events(uint16_t time_available_usec)
{
for (uint8_t i=0; i<NUM_TIMER_EVENTS; i++) {
if (tick_counter - timer_counters[i] >= pgm_read_word(&timer_events[i].time_interval_10ms)) {
// this event is due to run. Do we have enough time to run it?
event_time_allowed = pgm_read_word(&timer_events[i].min_time_usec);
if (event_time_allowed <= time_available_usec) {
// run it
event_time_started = micros();
event_fn_t func = (event_fn_t)pgm_read_pointer(&timer_events[i].func);
func();
// record the tick counter when we ran. This drives
// when we next run the event
timer_counters[i] = tick_counter;
// work out how long the event actually took
uint32_t time_taken = micros() - event_time_started;
if (time_taken > event_time_allowed) {
// the event overran!
#if SCHEDULER_DEBUG
cliSerial->printf_P(PSTR("overrun in event %u (%u/%u)\n"),
(unsigned)i,
(unsigned)time_taken,
(unsigned)event_time_allowed);
#endif
return;
}
time_available_usec -= time_taken;
}
}
}
}
void loop()
{
@ -944,17 +1066,7 @@ void loop()
// We want this to execute fast
// ----------------------------
if (main_loop_ready()) {
#if 0
uint16_t num_samples = ins.num_samples_available();
static uint16_t counter;
if (num_samples != 2 || (counter++ % 200 == 0)) {
cliSerial->printf_P(PSTR("num_samples=%u dt=%u\n"),
(unsigned)num_samples,
(unsigned)(timer - fast_loopTimer));
}
#endif
if (ins.num_samples_available() >= 2) {
#if DEBUG_FAST_LOOP == ENABLED
Log_Write_Data(DATA_FAST_LOOP, (int32_t)(timer - fast_loopTimer));
@ -963,8 +1075,8 @@ void loop()
// check loop time
perf_info_check_loop_time(timer - fast_loopTimer);
G_Dt = (float)(timer - fast_loopTimer) / 1000000.f; // used by PI Loops
fast_loopTimer = timer;
G_Dt = (float)(timer - fast_loopTimer) / 1000000.f; // used by PI Loops
fast_loopTimer = timer;
// for mainloop failure monitoring
mainLoop_count++;
@ -973,68 +1085,20 @@ void loop()
// ---------------------
fast_loop();
// run the 50hz loop 1/2 the time
ap_system.run_50hz_loop = !ap_system.run_50hz_loop;
if(ap_system.run_50hz_loop) {
#if DEBUG_MED_LOOP == ENABLED
Log_Write_Data(DATA_MED_LOOP, (int32_t)(timer - fiftyhz_loopTimer));
#endif
// store the micros for the 50 hz timer
fiftyhz_loopTimer = timer;
// check for new GPS messages
// --------------------------
update_GPS();
// run navigation routines
update_navigation();
// perform 10hz tasks
// ------------------
medium_loop();
// Stuff to run at full 50hz, but after the med loops
// --------------------------------------------------
fifty_hz_loop();
counter_one_herz++;
// trgger our 1 hz loop
if(counter_one_herz >= 50) {
super_slow_loop();
counter_one_herz = 0;
}
perf_mon_counter++;
if (perf_mon_counter >= 500 ) { // 500 iterations at 50hz = 10 seconds
if (g.log_bitmask & MASK_LOG_PM)
Log_Write_Performance();
perf_info_reset();
gps_fix_count = 0;
perf_mon_counter = 0;
}
}else{
// process communications with the GCS
gcs_check();
}
tick_counter++;
} else {
if (timer - fast_loopTimer < 9000) {
// we have some spare cycles available
// less than 10ms has passed. We have at least one millisecond
// of free time. The most useful thing to do with that time is
// to accumulate some sensor readings, specifically the
// compass, which is often very noisy but is not interrupt
// driven, so it can't accumulate readings by itself
if (g.compass_enabled) {
compass.accumulate();
}
uint16_t time_to_next_loop;
uint16_t dt = timer - fast_loopTimer;
if (dt > 10000) {
time_to_next_loop = 0;
} else {
time_to_next_loop = 10000 - dt;
}
run_events(time_to_next_loop);
}
}
// Main loop - 100hz
static void fast_loop()
{
@ -1078,6 +1142,7 @@ static void fast_loop()
// update targets to rate controllers
update_rate_contoller_targets();
// agmatthews - USERHOOKS
#ifdef USERHOOK_FASTLOOP
USERHOOK_FASTLOOP
#endif
@ -1178,6 +1243,7 @@ static void medium_loop()
// -----------------------
arm_motors();
// agmatthews - USERHOOKS
#ifdef USERHOOK_MEDIUMLOOP
USERHOOK_MEDIUMLOOP
#endif
@ -1185,8 +1251,6 @@ static void medium_loop()
#if COPTER_LEDS == ENABLED
update_copter_leds();
#endif
slow_loop();
break;
default:
@ -1201,10 +1265,6 @@ static void medium_loop()
// ---------------------------
static void fifty_hz_loop()
{
// read altitude sensors or estimate altitude
// ------------------------------------------
update_altitude_est();
// Update the throttle ouput
// -------------------------
update_throttle_mode();
@ -1307,6 +1367,7 @@ static void slow_loop()
camera_mount2.update_mount_type();
#endif
// agmatthews - USERHOOKS
#ifdef USERHOOK_SLOWLOOP
USERHOOK_SLOWLOOP
#endif
@ -1368,7 +1429,7 @@ static void super_slow_loop()
static void update_optical_flow(void)
{
static uint32_t last_of_update = 0;
static int log_counter = 0;
static uint8_t of_log_counter = 0;
// if new data has arrived, process it
if( optflow.last_update != last_of_update ) {
@ -1376,9 +1437,9 @@ static void update_optical_flow(void)
optflow.update_position(ahrs.roll, ahrs.pitch, cos_yaw_x, sin_yaw_y, current_loc.alt); // updates internal lon and lat with estimation based on optical flow
// write to log at 5hz
log_counter++;
if( log_counter >= 4 ) {
log_counter = 0;
of_log_counter++;
if( of_log_counter >= 4 ) {
of_log_counter = 0;
if (g.log_bitmask & MASK_LOG_OPTFLOW) {
Log_Write_Optflow();
}
@ -1772,16 +1833,21 @@ bool set_throttle_mode( uint8_t new_throttle_mode )
case THROTTLE_MANUAL:
case THROTTLE_MANUAL_TILT_COMPENSATED:
throttle_accel_deactivate(); // this controller does not use accel based throttle controller
altitude_error = 0; // clear altitude error reported to GCS
throttle_initialised = true;
break;
case THROTTLE_ACCELERATION: // pilot inputs the desired acceleration
if( g.throttle_accel_enabled ) { // this throttle mode requires use of the accel based throttle controller
altitude_error = 0; // clear altitude error reported to GCS
throttle_initialised = true;
}
break;
case THROTTLE_RATE:
altitude_error = 0; // clear altitude error reported to GCS
throttle_initialised = true;
break;
case THROTTLE_STABILIZED_RATE:
case THROTTLE_DIRECT_ALT:
throttle_initialised = true;
@ -1850,7 +1916,7 @@ void update_throttle_mode(void)
}
#if FRAME_CONFIG == HELI_FRAME
if (roll_pitch_mode == ROLL_PITCH_STABLE){
if (control_mode == STABILIZE){
motors.stab_throttle = true;
} else {
motors.stab_throttle = false;
@ -1935,6 +2001,7 @@ void update_throttle_mode(void)
if(g.rc_3.control_in <= 0){
set_throttle_out(0, false);
throttle_accel_deactivate(); // do not allow the accel based throttle to override our command
altitude_error = 0; // clear altitude error reported to GCS - normally underlying alt hold controller updates altitude error reported to GCS
}else{
pilot_climb_rate = get_pilot_desired_climb_rate(g.rc_3.control_in);
get_throttle_rate_stabilized(pilot_climb_rate);
@ -1946,8 +2013,8 @@ void update_throttle_mode(void)
if(g.rc_3.control_in <= 0){
set_throttle_out(0, false);
throttle_accel_deactivate(); // do not allow the accel based throttle to override our command
altitude_error = 0; // clear altitude error reported to GCS - normally underlying alt hold controller updates altitude error reported to GCS
}else{
// To-Do: this should update the global desired altitude variable next_WP.alt
int32_t desired_alt = get_pilot_desired_direct_alt(g.rc_3.control_in);
get_throttle_althold(desired_alt, g.auto_velocity_z_min, g.auto_velocity_z_max);
}
@ -1991,7 +2058,7 @@ static void read_AHRS(void)
//-----------------------------------------------
#if HIL_MODE != HIL_MODE_DISABLED
// update hil before ahrs update
gcs_update();
gcs_check_input();
#endif
ahrs.update();

103
ArduCopter/Attitude.pde
View File

@ -74,12 +74,11 @@ get_stabilize_yaw(int32_t target_angle)
#endif
#if LOGGING_ENABLED == ENABLED
static int8_t log_counter = 0; // used to slow down logging of PID values to dataflash
// log output if PID logging is on and we are tuning the yaw
if( g.log_bitmask & MASK_LOG_PID && (g.radio_tuning == CH6_YAW_KP || g.radio_tuning == CH6_YAW_RATE_KP) ) {
log_counter++;
if( log_counter >= 10 ) { // (update rate / desired output rate) = (100hz / 10hz) = 10
log_counter = 0;
if( g.log_bitmask & MASK_LOG_PID && g.radio_tuning == CH6_YAW_KP ) {
pid_log_counter++;
if( pid_log_counter >= 10 ) { // (update rate / desired output rate) = (100hz / 10hz) = 10
pid_log_counter = 0;
Log_Write_PID(CH6_YAW_KP, angle_error, target_rate, i_term, 0, output, tuning_value);
}
}
@ -333,13 +332,11 @@ get_heli_rate_roll(int32_t target_rate)
output = constrain(output, -4500, 4500);
#if LOGGING_ENABLED == ENABLED
static int8_t log_counter = 0; // used to slow down logging of PID values to dataflash
// log output if PID logging is on and we are tuning the rate P, I or D gains
if( g.log_bitmask & MASK_LOG_PID && (g.radio_tuning == CH6_RATE_KP || g.radio_tuning == CH6_RATE_KI || g.radio_tuning == CH6_RATE_KD) ) {
log_counter++;
if( log_counter >= 10 ) { // (update rate / desired output rate) = (100hz / 10hz) = 10
log_counter = 0;
pid_log_counter++;
if( pid_log_counter >= 10 ) { // (update rate / desired output rate) = (100hz / 10hz) = 10
pid_log_counter = 0;
Log_Write_PID(CH6_RATE_KP, rate_error, p, i, d, output, tuning_value);
}
}
@ -387,12 +384,9 @@ get_heli_rate_pitch(int32_t target_rate)
output = constrain(output, -4500, 4500);
#if LOGGING_ENABLED == ENABLED
static int8_t log_counter = 0; // used to slow down logging of PID values to dataflash
// log output if PID logging is on and we are tuning the rate P, I or D gains
if( g.log_bitmask & MASK_LOG_PID && (g.radio_tuning == CH6_RATE_KP || g.radio_tuning == CH6_RATE_KI || g.radio_tuning == CH6_RATE_KD) ) {
log_counter++;
if( log_counter >= 10 ) { // (update rate / desired output rate) = (100hz / 10hz) = 10
log_counter = 0;
if( pid_log_counter == 0 ) { // relies on get_heli_rate_roll to update pid_log_counter
Log_Write_PID(CH6_RATE_KP+100, rate_error, p, i, 0, output, tuning_value);
}
}
@ -432,12 +426,11 @@ get_heli_rate_yaw(int32_t target_rate)
output = constrain(output, -4500, 4500);
#if LOGGING_ENABLED == ENABLED
static int8_t log_counter = 0; // used to slow down logging of PID values to dataflash
// log output if PID loggins is on and we are tuning the yaw
if( g.log_bitmask & MASK_LOG_PID && (g.radio_tuning == CH6_YAW_KP || g.radio_tuning == CH6_YAW_RATE_KP) ) {
log_counter++;
if( log_counter >= 10 ) { // (update rate / desired output rate) = (100hz / 10hz) = 10
log_counter = 0;
if( g.log_bitmask & MASK_LOG_PID && (g.radio_tuning == CH6_YAW_RATE_KP || g.radio_tuning == CH6_YAW_RATE_KD) ) {
pid_log_counter++;
if( pid_log_counter >= 10 ) { // (update rate / desired output rate) = (100hz / 10hz) = 10
pid_log_counter = 0;
Log_Write_PID(CH6_YAW_RATE_KP, rate_error, p, i, d, output, tuning_value);
}
}
@ -479,14 +472,12 @@ get_rate_roll(int32_t target_rate)
output = constrain(output, -5000, 5000);
#if LOGGING_ENABLED == ENABLED
static int8_t log_counter = 0; // used to slow down logging of PID values to dataflash
// log output if PID logging is on and we are tuning the rate P, I or D gains
if( g.log_bitmask & MASK_LOG_PID && (g.radio_tuning == CH6_RATE_KP || g.radio_tuning == CH6_RATE_KI || g.radio_tuning == CH6_RATE_KD) ) {
log_counter++;
if( log_counter >= 10 ) { // (update rate / desired output rate) = (100hz / 10hz) = 10
log_counter = 0;
Log_Write_PID(CH6_RATE_KP, rate_error, p, i, d /*-rate_d_dampener*/, output, tuning_value);
pid_log_counter++; // Note: get_rate_pitch pid logging relies on this function to update pid_log_counter so if you change the line above you must change the equivalent line in get_rate_pitch
if( pid_log_counter >= 10 ) { // (update rate / desired output rate) = (100hz / 10hz) = 10
pid_log_counter = 0;
Log_Write_PID(CH6_RATE_KP, rate_error, p, i, d, output, tuning_value);
}
}
#endif
@ -522,13 +513,10 @@ get_rate_pitch(int32_t target_rate)
output = constrain(output, -5000, 5000);
#if LOGGING_ENABLED == ENABLED
static int8_t log_counter = 0; // used to slow down logging of PID values to dataflash
// log output if PID logging is on and we are tuning the rate P, I or D gains
if( g.log_bitmask & MASK_LOG_PID && (g.radio_tuning == CH6_RATE_KP || g.radio_tuning == CH6_RATE_KI || g.radio_tuning == CH6_RATE_KD) ) {
log_counter++;
if( log_counter >= 10 ) { // (update rate / desired output rate) = (100hz / 10hz) = 10
log_counter = 0;
Log_Write_PID(CH6_RATE_KP+100, rate_error, p, i, d/*-rate_d_dampener*/, output, tuning_value);
if( pid_log_counter == 0 ) { // relies on get_rate_roll having updated pid_log_counter
Log_Write_PID(CH6_RATE_KP+100, rate_error, p, i, d, output, tuning_value);
}
}
#endif
@ -561,12 +549,11 @@ get_rate_yaw(int32_t target_rate)
output = constrain(output, -4500, 4500);
#if LOGGING_ENABLED == ENABLED
static int8_t log_counter = 0; // used to slow down logging of PID values to dataflash
// log output if PID loggins is on and we are tuning the yaw
if( g.log_bitmask & MASK_LOG_PID && (g.radio_tuning == CH6_YAW_KP || g.radio_tuning == CH6_YAW_RATE_KP) ) {
log_counter++;
if( log_counter >= 10 ) { // (update rate / desired output rate) = (100hz / 10hz) = 10
log_counter = 0;
if( g.log_bitmask & MASK_LOG_PID && g.radio_tuning == CH6_YAW_RATE_KP ) {
pid_log_counter++;
if( pid_log_counter >= 10 ) { // (update rate / desired output rate) = (100hz / 10hz) = 10
pid_log_counter = 0;
Log_Write_PID(CH6_YAW_RATE_KP, rate_error, p, i, d, output, tuning_value);
}
}
@ -619,12 +606,11 @@ get_of_roll(int32_t input_roll)
of_roll = constrain(new_roll, (of_roll-20), (of_roll+20));
#if LOGGING_ENABLED == ENABLED
static int8_t log_counter = 0; // used to slow down logging of PID values to dataflash
// log output if PID logging is on and we are tuning the rate P, I or D gains
if( g.log_bitmask & MASK_LOG_PID && (g.radio_tuning == CH6_OPTFLOW_KP || g.radio_tuning == CH6_OPTFLOW_KI || g.radio_tuning == CH6_OPTFLOW_KD) ) {
log_counter++;
if( log_counter >= 5 ) { // (update rate / desired output rate) = (100hz / 10hz) = 10
log_counter = 0;
pid_log_counter++; // Note: get_of_pitch pid logging relies on this function updating pid_log_counter so if you change the line above you must change the equivalent line in get_of_pitch
if( pid_log_counter >= 5 ) { // (update rate / desired output rate) = (100hz / 10hz) = 10
pid_log_counter = 0;
Log_Write_PID(CH6_OPTFLOW_KP, tot_x_cm, p, i, d, of_roll, tuning_value);
}
}
@ -674,12 +660,9 @@ get_of_pitch(int32_t input_pitch)
of_pitch = constrain(new_pitch, (of_pitch-20), (of_pitch+20));
#if LOGGING_ENABLED == ENABLED
static int8_t log_counter = 0; // used to slow down logging of PID values to dataflash
// log output if PID logging is on and we are tuning the rate P, I or D gains
if( g.log_bitmask & MASK_LOG_PID && (g.radio_tuning == CH6_OPTFLOW_KP || g.radio_tuning == CH6_OPTFLOW_KI || g.radio_tuning == CH6_OPTFLOW_KD) ) {
log_counter++;
if( log_counter >= 5 ) { // (update rate / desired output rate) = (100hz / 10hz) = 10
log_counter = 0;
if( pid_log_counter == 0 ) { // relies on get_of_roll having updated the pid_log_counter
Log_Write_PID(CH6_OPTFLOW_KP+100, tot_y_cm, p, i, d, of_pitch, tuning_value);
}
}
@ -829,13 +812,12 @@ get_throttle_accel(int16_t z_target_accel)
output = constrain(p+i+d+g.throttle_cruise, g.throttle_min, g.throttle_max);
#if LOGGING_ENABLED == ENABLED
static int8_t log_counter = 0; // used to slow down logging of PID values to dataflash
// log output if PID loggins is on and we are tuning the yaw
if( g.log_bitmask & MASK_LOG_PID && (g.radio_tuning == CH6_THR_ACCEL_KD || g.radio_tuning == CH6_THROTTLE_KP) ) {
log_counter++;
if( log_counter >= 10 ) { // (update rate / desired output rate) = (50hz / 10hz) = 5hz
log_counter = 0;
Log_Write_PID(CH6_THR_ACCEL_KD, z_accel_error, p, i, d, output, tuning_value);
if( g.log_bitmask & MASK_LOG_PID && (g.radio_tuning == CH6_THR_ACCEL_KP || g.radio_tuning == CH6_THR_ACCEL_KI || g.radio_tuning == CH6_THR_ACCEL_KD) ) {
pid_log_counter++;
if( pid_log_counter >= 10 ) { // (update rate / desired output rate) = (50hz / 10hz) = 5hz
pid_log_counter = 0;
Log_Write_PID(CH6_THR_ACCEL_KP, z_accel_error, p, i, d, output, tuning_value);
}
}
#endif
@ -956,12 +938,11 @@ get_throttle_rate(int16_t z_target_speed)
output = p+i+d;
#if LOGGING_ENABLED == ENABLED
static uint8_t log_counter = 0; // used to slow down logging of PID values to dataflash
// log output if PID loggins is on and we are tuning the yaw
if( g.log_bitmask & MASK_LOG_PID && (g.radio_tuning == CH6_THROTTLE_KP || g.radio_tuning == CH6_THROTTLE_KI || g.radio_tuning == CH6_THROTTLE_KD) ) {
log_counter++;
if( log_counter >= 10 ) { // (update rate / desired output rate) = (50hz / 10hz) = 5hz
log_counter = 0;
pid_log_counter++;
if( pid_log_counter >= 10 ) { // (update rate / desired output rate) = (50hz / 10hz) = 5hz
pid_log_counter = 0;
Log_Write_PID(CH6_THROTTLE_KP, z_rate_error, p, i, d, output, tuning_value);
}
}
@ -988,21 +969,22 @@ get_throttle_rate(int16_t z_target_speed)
static void
get_throttle_althold(int32_t target_alt, int16_t min_climb_rate, int16_t max_climb_rate)
{
int32_t alt_error;
int16_t desired_rate;
int32_t linear_distance; // the distace we swap between linear and sqrt.
// calculate altitude error
altitude_error = target_alt - current_loc.alt;
alt_error = target_alt - current_loc.alt;
// check kP to avoid division by zero
if( g.pi_alt_hold.kP() != 0 ) {
linear_distance = 250/(2*g.pi_alt_hold.kP()*g.pi_alt_hold.kP());
if( altitude_error > 2*linear_distance ) {
desired_rate = sqrt(2*250*(altitude_error-linear_distance));
}else if( altitude_error < -2*linear_distance ) {
desired_rate = -sqrt(2*250*(-altitude_error-linear_distance));
if( alt_error > 2*linear_distance ) {
desired_rate = sqrt(2*250*(alt_error-linear_distance));
}else if( alt_error < -2*linear_distance ) {
desired_rate = -sqrt(2*250*(-alt_error-linear_distance));
}else{
desired_rate = g.pi_alt_hold.get_p(altitude_error);
desired_rate = g.pi_alt_hold.get_p(alt_error);
}
}else{
desired_rate = 0;
@ -1013,6 +995,9 @@ get_throttle_althold(int32_t target_alt, int16_t min_climb_rate, int16_t max_cli
// call rate based throttle controller which will update accel based throttle controller targets
get_throttle_rate(desired_rate);
// update altitude error reported to GCS
altitude_error = alt_error;
// TO-DO: enabled PID logging for this controller
}

88
ArduCopter/GCS_Mavlink.pde
View File

@ -11,6 +11,9 @@ static mavlink_statustext_t pending_status;
// true when we have received at least 1 MAVLink packet
static bool mavlink_active;
// true if we are out of time in our event timeslice
static bool gcs_out_of_time;
// check if a message will fit in the payload space available
#define CHECK_PAYLOAD_SIZE(id) if (payload_space < MAVLINK_MSG_ID_ ## id ## _LEN) return false
@ -18,30 +21,35 @@ static bool mavlink_active;
// prototype this for use inside the GCS class
static void gcs_send_text_fmt(const prog_char_t *fmt, ...);
// gcs_check - throttles communication with ground station.
// should be called regularly
// returns true if it has sent a message to the ground station
static bool gcs_check()
// gcs_check_delay - keep GCS communications going
// in our delay callback
static void gcs_check_delay()
{
static uint32_t last_1hz, last_50hz;
bool sent_message = false;
uint32_t tnow = millis();
if (tnow - last_1hz > 1000) {
last_1hz = tnow;
gcs_send_message(MSG_HEARTBEAT);
sent_message = true;
gcs_send_heartbeat();
}
if (tnow - last_50hz > 20 && !sent_message) {
if (tnow - last_50hz > 20) {
last_50hz = tnow;
gcs_update();
gcs_check_input();
gcs_data_stream_send();
sent_message = true;
gcs_send_deferred();
}
}
gcs_send_message(MSG_RETRY_DEFERRED);
static void gcs_send_heartbeat(void)
{
gcs_send_message(MSG_HEARTBEAT);
}
return sent_message;
static void gcs_send_deferred(void)
{
gcs_send_message(MSG_RETRY_DEFERRED);
}
/*
@ -483,18 +491,6 @@ static void NOINLINE send_raw_imu3(mavlink_channel_t chan)
accel_offsets.z);
}
static void NOINLINE send_gps_status(mavlink_channel_t chan)
{
mavlink_msg_gps_status_send(
chan,
g_gps->num_sats,
NULL,
NULL,
NULL,
NULL,
NULL);
}
static void NOINLINE send_current_waypoint(mavlink_channel_t chan)
{
mavlink_msg_mission_current_send(
@ -545,10 +541,11 @@ static bool mavlink_try_send_message(mavlink_channel_t chan, enum ap_message id,
return false;
}
// if the ins has new data for the main loop then don't send a
// mavlink message. We want to prioritise the main flight control
// loop over communications
if (main_loop_ready()) {
// if we don't have at least 1ms remaining before the main loop
// wants to fire then don't send a mavlink message. We want to
// prioritise the main flight control loop over communications
if (event_time_available() < 500) {
gcs_out_of_time = true;
return false;
}
@ -556,7 +553,7 @@ static bool mavlink_try_send_message(mavlink_channel_t chan, enum ap_message id,
case MSG_HEARTBEAT:
CHECK_PAYLOAD_SIZE(HEARTBEAT);
send_heartbeat(chan);
return true;
break;
case MSG_EXTENDED_STATUS1:
CHECK_PAYLOAD_SIZE(SYS_STATUS);
@ -680,6 +677,7 @@ static bool mavlink_try_send_message(mavlink_channel_t chan, enum ap_message id,
case MSG_RETRY_DEFERRED:
break; // just here to prevent a warning
}
return true;
}
@ -968,6 +966,8 @@ GCS_MAVLINK::data_stream_send(void)
return;
}
gcs_out_of_time = false;
if (_queued_parameter != NULL) {
if (streamRateParams.get() <= 0) {
streamRateParams.set(50);
@ -979,6 +979,8 @@ GCS_MAVLINK::data_stream_send(void)
return;
}
if (gcs_out_of_time) return;
if (in_mavlink_delay) {
// don't send any other stream types while in the delay callback
return;
@ -991,6 +993,8 @@ GCS_MAVLINK::data_stream_send(void)
//cliSerial->printf("mav1 %d\n", (int)streamRateRawSensors.get());
}
if (gcs_out_of_time) return;
if (stream_trigger(STREAM_EXTENDED_STATUS)) {
send_message(MSG_EXTENDED_STATUS1);
send_message(MSG_EXTENDED_STATUS2);
@ -1000,32 +1004,44 @@ GCS_MAVLINK::data_stream_send(void)
send_message(MSG_LIMITS_STATUS);
}
if (gcs_out_of_time) return;
if (stream_trigger(STREAM_POSITION)) {
send_message(MSG_LOCATION);
}
if (gcs_out_of_time) return;
if (stream_trigger(STREAM_RAW_CONTROLLER)) {
send_message(MSG_SERVO_OUT);
//cliSerial->printf("mav4 %d\n", (int)streamRateRawController.get());
}
if (gcs_out_of_time) return;
if (stream_trigger(STREAM_RC_CHANNELS)) {
send_message(MSG_RADIO_OUT);
send_message(MSG_RADIO_IN);
//cliSerial->printf("mav5 %d\n", (int)streamRateRCChannels.get());
}
if (gcs_out_of_time) return;
if (stream_trigger(STREAM_EXTRA1)) {
send_message(MSG_ATTITUDE);
send_message(MSG_SIMSTATE);
//cliSerial->printf("mav6 %d\n", (int)streamRateExtra1.get());
}
if (gcs_out_of_time) return;
if (stream_trigger(STREAM_EXTRA2)) {
send_message(MSG_VFR_HUD);
//cliSerial->printf("mav7 %d\n", (int)streamRateExtra2.get());
}
if (gcs_out_of_time) return;
if (stream_trigger(STREAM_EXTRA3)) {
send_message(MSG_AHRS);
send_message(MSG_HWSTATUS);
@ -1452,21 +1468,22 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
if (mavlink_check_target(packet.target_system,packet.target_component)) break;
enum ap_var_type p_type;
AP_Param *vp;
char param_name[AP_MAX_NAME_SIZE];
if (packet.param_index != -1) {
vp = AP_Param::find_by_index(packet.param_index, &p_type);
if (vp == NULL) {
gcs_send_text_fmt(PSTR("Unknown parameter index %d"), packet.param_index);
break;
}
vp->copy_name(param_name, sizeof(param_name), true);
} else {
vp = AP_Param::find(packet.param_id, &p_type);
if (vp == NULL) {
gcs_send_text_fmt(PSTR("Unknown parameter %.16s"), packet.param_id);
break;
}
strncpy(param_name, packet.param_id, AP_MAX_NAME_SIZE);
}
char param_name[AP_MAX_NAME_SIZE];
vp->copy_name(param_name, sizeof(param_name), true);
float value = vp->cast_to_float(p_type);
mavlink_msg_param_value_send(
@ -2090,7 +2107,7 @@ GCS_MAVLINK::queued_param_send()
value = vp->cast_to_float(_queued_parameter_type);
char param_name[AP_MAX_NAME_SIZE];
vp->copy_name(param_name, sizeof(param_name), true);
vp->copy_name_token(&_queued_parameter_token, param_name, sizeof(param_name), true);
mavlink_msg_param_value_send(
chan,
@ -2138,13 +2155,14 @@ static void mavlink_delay_cb()
uint32_t tnow = millis();
if (tnow - last_1hz > 1000) {
last_1hz = tnow;
gcs_send_message(MSG_HEARTBEAT);
gcs_send_heartbeat();
gcs_send_message(MSG_EXTENDED_STATUS1);
}
if (tnow - last_50hz > 20) {
last_50hz = tnow;
gcs_update();
gcs_check_input();
gcs_data_stream_send();
gcs_send_deferred();
}
if (tnow - last_5s > 5000) {
last_5s = tnow;
@ -2182,7 +2200,7 @@ static void gcs_data_stream_send(void)
/*
* look for incoming commands on the GCS links
*/
static void gcs_update(void)
static void gcs_check_input(void)
{
gcs0.update();
if (gcs3.initialised) {

40
ArduCopter/navigation.pde
View File

@ -41,20 +41,14 @@ static void update_navigation()
}
#endif
// calc various navigation values and run controllers if we've received a position update
// setup to calculate new navigation values and run controllers if
// we've received a position update
if( pos_updated ) {
// calculate velocity
calc_velocity_and_position();
// calculate distance, angles to target
calc_distance_and_bearing();
// run navigation controllers
run_navigation_contollers();
// Rotate the nav_lon and nav_lat vectors based on Yaw
calc_nav_pitch_roll();
nav_updates.need_velpos = 1;
nav_updates.need_dist_bearing = 1;
nav_updates.need_nav_controllers = 1;
nav_updates.need_nav_pitch_roll = 1;
// update log
if (log_output && (g.log_bitmask & MASK_LOG_NTUN) && motors.armed()) {
@ -71,6 +65,28 @@ static void update_navigation()
}
}
/*
run navigation updates from nav_updates. Only run one at a time to
prevent too much cpu usage hurting the main loop
*/
static void run_nav_updates(void)
{
if (nav_updates.need_velpos) {
calc_velocity_and_position();
nav_updates.need_velpos = 0;
} else if (nav_updates.need_dist_bearing) {
calc_distance_and_bearing();
nav_updates.need_dist_bearing = 0;
} else if (nav_updates.need_nav_controllers) {
run_navigation_contollers();
nav_updates.need_nav_controllers = 0;
} else if (nav_updates.need_nav_pitch_roll) {
calc_nav_pitch_roll();
nav_updates.need_nav_pitch_roll = 0;
}
}
//*******************************************************************************************************
// calc_velocity_and_filtered_position - velocity in lon and lat directions calculated from GPS position
// and accelerometer data