mirror of https://github.com/ArduPilot/ardupilot
adapted main ACM code for AP_Param
This commit is contained in:
parent
983192c5d5
commit
3b4cae3cc3
|
@ -219,7 +219,7 @@ AP_InertialSensor_MPU6000 ins( CONFIG_MPU6000_CHIP_SELECT_PIN );
|
||||||
#else
|
#else
|
||||||
AP_InertialSensor_Oilpan ins(&adc);
|
AP_InertialSensor_Oilpan ins(&adc);
|
||||||
#endif
|
#endif
|
||||||
AP_IMU_INS imu(&ins, Parameters::k_param_IMU_calibration);
|
AP_IMU_INS imu(&ins);
|
||||||
AP_DCM dcm(&imu, g_gps);
|
AP_DCM dcm(&imu, g_gps);
|
||||||
AP_TimerProcess timer_scheduler;
|
AP_TimerProcess timer_scheduler;
|
||||||
|
|
||||||
|
@ -258,8 +258,8 @@ AP_TimerProcess timer_scheduler;
|
||||||
////////////////////////////////////////////////////////////////////////////////
|
////////////////////////////////////////////////////////////////////////////////
|
||||||
// GCS selection
|
// GCS selection
|
||||||
////////////////////////////////////////////////////////////////////////////////
|
////////////////////////////////////////////////////////////////////////////////
|
||||||
GCS_MAVLINK gcs0(Parameters::k_param_streamrates_port0);
|
GCS_MAVLINK gcs0;
|
||||||
GCS_MAVLINK gcs3(Parameters::k_param_streamrates_port3);
|
GCS_MAVLINK gcs3;
|
||||||
|
|
||||||
////////////////////////////////////////////////////////////////////////////////
|
////////////////////////////////////////////////////////////////////////////////
|
||||||
// SONAR selection
|
// SONAR selection
|
||||||
|
@ -1919,7 +1919,7 @@ static void tuning(){
|
||||||
|
|
||||||
case CH6_DAMP:
|
case CH6_DAMP:
|
||||||
g.rc_6.set_range(0,300); // 0 to 1
|
g.rc_6.set_range(0,300); // 0 to 1
|
||||||
g.stablize_d.set(tuning_value);
|
g.stabilize_d.set(tuning_value);
|
||||||
|
|
||||||
//tuning_value = (float)g.rc_6.control_in / 100000.0;
|
//tuning_value = (float)g.rc_6.control_in / 100000.0;
|
||||||
//g.rc_6.set_range(0,1000); // 0 to 1
|
//g.rc_6.set_range(0,1000); // 0 to 1
|
||||||
|
|
|
@ -115,7 +115,7 @@ get_rate_roll(int32_t target_rate)
|
||||||
target_rate = g.pid_rate_roll.get_pid(target_rate, G_Dt);
|
target_rate = g.pid_rate_roll.get_pid(target_rate, G_Dt);
|
||||||
|
|
||||||
// Dampening
|
// 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;
|
last_rate = current_rate;
|
||||||
|
|
||||||
// output control:
|
// 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);
|
target_rate = g.pid_rate_pitch.get_pid(target_rate, G_Dt);
|
||||||
|
|
||||||
// Dampening
|
// 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;
|
last_rate = current_rate;
|
||||||
|
|
||||||
// output control:
|
// output control:
|
||||||
|
|
|
@ -102,7 +102,34 @@ setup_show(uint8_t argc, const Menu::arg *argv)
|
||||||
report_gyro();
|
report_gyro();
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
AP_Var_menu_show(argc, argv);
|
uint32_t token;
|
||||||
|
AP_Param *ap;
|
||||||
|
enum ap_var_type type;
|
||||||
|
|
||||||
|
for (ap=AP_Param::first(&token, &type);
|
||||||
|
ap;
|
||||||
|
ap=AP_Param::next_scalar(&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("%s: %d\n", s, (int)((AP_Int8 *)ap)->get());
|
||||||
|
break;
|
||||||
|
case AP_PARAM_INT16:
|
||||||
|
Serial.printf_P("%s: %d\n", s, (int)((AP_Int16 *)ap)->get());
|
||||||
|
break;
|
||||||
|
case AP_PARAM_INT32:
|
||||||
|
Serial.printf_P("%s: %ld\n", s, (long)((AP_Int32 *)ap)->get());
|
||||||
|
break;
|
||||||
|
case AP_PARAM_FLOAT:
|
||||||
|
Serial.printf_P("%s: %f\n", s, ((AP_Float *)ap)->get());
|
||||||
|
break;
|
||||||
|
default:
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
return(0);
|
return(0);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -122,7 +149,7 @@ setup_factory(uint8_t argc, const Menu::arg *argv)
|
||||||
if (('y' != c) && ('Y' != c))
|
if (('y' != c) && ('Y' != c))
|
||||||
return(-1);
|
return(-1);
|
||||||
|
|
||||||
AP_Var::erase_all();
|
AP_Param::erase_all();
|
||||||
Serial.printf_P(PSTR("\nReboot APM"));
|
Serial.printf_P(PSTR("\nReboot APM"));
|
||||||
|
|
||||||
delay(1000);
|
delay(1000);
|
||||||
|
|
|
@ -159,34 +159,8 @@ static void init_ardupilot()
|
||||||
piezo_beep();
|
piezo_beep();
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
// load parameters from EEPROM
|
||||||
if (!g.format_version.load() ||
|
load_parameters();
|
||||||
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();
|
|
||||||
}
|
|
||||||
|
|
||||||
// init the GCS
|
// init the GCS
|
||||||
gcs0.init(&Serial);
|
gcs0.init(&Serial);
|
||||||
|
|
Loading…
Reference in New Issue