static void show_data()
{
float result[6];
uint32_t deltat = 0;
uint16_t ch3;
float min[6], max[6];
uint8_t i;
for (i=0; i<6; i++) {
/* clearly out of bounds values: */
min[i] = 99999999.0f;
max[i] = -88888888.0f;
}
do {
ch3 = adc.Ch(3);
deltat += adc.Ch6(channel_map, result);
for (i=0; i<6; i++) {
if (result[i] < min[i]) min[i] = result[i];
if (result[i] > max[i]) max[i] = result[i];
if (fabsf(result[i]) > 0x8000) {
hal.console->printf("result[%u]=%f\n", (unsigned)i, result[i]);
}
}
} while ((AP_HAL::millis() - timer) < 200);
timer = AP_HAL::millis();
hal.console->printf("g=(%f,%f,%f) a=(%f,%f,%f) +/-(%.0f,%.0f,%.0f,%.0f,%.0f,%.0f) gt=%u dt=%u\n",
result[0], result[1], result[2],
result[3], result[4], result[5],
(max[0]-min[0]), (max[1]-min[1]), (max[2]-min[2]),
(max[3]-min[3]), (max[4]-min[4]), (max[5]-min[5]),
ch3, (unsigned)(deltat/1000));
}
bool AP_InertialSensor_Oilpan::_init_sensor(void)
{
apm1_adc.Init();
switch (_imu.get_sample_rate()) {
case AP_InertialSensor::RATE_50HZ:
_sample_threshold = 20;
break;
case AP_InertialSensor::RATE_100HZ:
_sample_threshold = 10;
break;
case AP_InertialSensor::RATE_200HZ:
_sample_threshold = 5;
break;
default:
// can't do this speed
return false;
}
_gyro_instance = _imu.register_gyro();
_accel_instance = _imu.register_accel();
_product_id = AP_PRODUCT_ID_APM1_2560;
return true;
}
void setup()
{
hal.console->println("ArduPilot Mega ADC library test");
hal.scheduler->delay(1000);
adc.Init(); // APM ADC initialization
hal.scheduler->delay(1000);
timer = AP_HAL::millis();
}
static void show_timing()
{
uint32_t mint = (uint32_t)-1, maxt = 0, totalt=0;
uint32_t start_time = AP_HAL::millis();
float result[6];
uint32_t count = 0;
hal.console->println("Starting timing test");
adc.Ch6(channel_map, result);
do {
uint32_t deltat = adc.Ch6(channel_map, result);
if (deltat > maxt) maxt = deltat;
if (deltat < mint) mint = deltat;
totalt += deltat;
count++;
} while ((AP_HAL::millis() - start_time) < 5000);
hal.console->printf("timing: mint=%lu maxt=%lu avg=%lu\n", mint, maxt, totalt/count);
}
/*
copy data from ADC to frontend
*/
bool AP_InertialSensor_Oilpan::update()
{
float adc_values[6];
apm1_adc.Ch6(_sensors, adc_values);
// copy gyros to frontend
Vector3f v;
v(_sensor_signs[0] * ( adc_values[0] - OILPAN_RAW_GYRO_OFFSET ) * _gyro_gain_x,
_sensor_signs[1] * ( adc_values[1] - OILPAN_RAW_GYRO_OFFSET ) * _gyro_gain_y,
_sensor_signs[2] * ( adc_values[2] - OILPAN_RAW_GYRO_OFFSET ) * _gyro_gain_z);
_rotate_and_correct_gyro(_gyro_instance, v);
_publish_gyro(_gyro_instance, v);
// copy accels to frontend
v(_sensor_signs[3] * (adc_values[3] - OILPAN_RAW_ACCEL_OFFSET),
_sensor_signs[4] * (adc_values[4] - OILPAN_RAW_ACCEL_OFFSET),
_sensor_signs[5] * (adc_values[5] - OILPAN_RAW_ACCEL_OFFSET));
v *= OILPAN_ACCEL_SCALE_1G;
_rotate_and_correct_accel(_accel_instance, v);
_publish_accel(_accel_instance, v);
return true;
}
void Plane::init_ardupilot()
{
// initialise serial port
serial_manager.init_console();
cliSerial->printf_P(PSTR("\n\nInit " FIRMWARE_STRING
"\n\nFree RAM: %u\n"),
hal.util->available_memory());
//
// Check the EEPROM format version before loading any parameters from EEPROM
//
load_parameters();
if (g.hil_mode == 1) {
// set sensors to HIL mode
ins.set_hil_mode();
compass.set_hil_mode();
barometer.set_hil_mode();
}
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
// this must be before BoardConfig.init() so if
// BRD_SAFETYENABLE==0 then we don't have safety off yet
for (uint8_t tries=0; tries<10; tries++) {
if (setup_failsafe_mixing()) {
break;
}
hal.scheduler->delay(10);
}
#endif
BoardConfig.init();
// initialise serial ports
serial_manager.init();
// allow servo set on all channels except first 4
ServoRelayEvents.set_channel_mask(0xFFF0);
set_control_channels();
// keep a record of how many resets have happened. This can be
// used to detect in-flight resets
g.num_resets.set_and_save(g.num_resets+1);
// init baro before we start the GCS, so that the CLI baro test works
barometer.init();
// initialise rangefinder
init_rangefinder();
// initialise battery monitoring
battery.init();
// init the GCS
gcs[0].setup_uart(serial_manager, AP_SerialManager::SerialProtocol_Console, 0);
// we start by assuming USB connected, as we initialed the serial
// port with SERIAL0_BAUD. check_usb_mux() fixes this if need be.
usb_connected = true;
check_usb_mux();
// setup serial port for telem1
gcs[1].setup_uart(serial_manager, AP_SerialManager::SerialProtocol_MAVLink, 0);
#if MAVLINK_COMM_NUM_BUFFERS > 2
// setup serial port for telem2
gcs[2].setup_uart(serial_manager, AP_SerialManager::SerialProtocol_MAVLink, 1);
#endif
#if MAVLINK_COMM_NUM_BUFFERS > 3
// setup serial port for fourth telemetry port (not used by default)
gcs[3].setup_uart(serial_manager, AP_SerialManager::SerialProtocol_MAVLink, 2);
#endif
// setup frsky
#if FRSKY_TELEM_ENABLED == ENABLED
frsky_telemetry.init(serial_manager);
#endif
mavlink_system.sysid = g.sysid_this_mav;
#if LOGGING_ENABLED == ENABLED
log_init();
#endif
#if CONFIG_HAL_BOARD == HAL_BOARD_APM1
apm1_adc.Init(); // APM ADC library initialization
#endif
// initialise airspeed sensor
airspeed.init();
if (g.compass_enabled==true) {
if (!compass.init() || !compass.read()) {
cliSerial->println_P(PSTR("Compass initialisation failed!"));
g.compass_enabled = false;
} else {
ahrs.set_compass(&compass);
}
}
#if OPTFLOW == ENABLED
// make optflow available to libraries
ahrs.set_optflow(&optflow);
#endif
// Register mavlink_delay_cb, which will run anytime you have
// more than 5ms remaining in your call to hal.scheduler->delay
hal.scheduler->register_delay_callback(mavlink_delay_cb_static, 5);
// give AHRS the airspeed sensor
ahrs.set_airspeed(&airspeed);
// GPS Initialization
gps.init(&DataFlash, serial_manager);
init_rc_in(); // sets up rc channels from radio
init_rc_out(); // sets up the timer libs
relay.init();
#if MOUNT == ENABLED
// initialise camera mount
camera_mount.init(serial_manager);
#endif
#if FENCE_TRIGGERED_PIN > 0
hal.gpio->pinMode(FENCE_TRIGGERED_PIN, HAL_GPIO_OUTPUT);
hal.gpio->write(FENCE_TRIGGERED_PIN, 0);
#endif
/*
* setup the 'main loop is dead' check. Note that this relies on
* the RC library being initialised.
*/
hal.scheduler->register_timer_failsafe(failsafe_check_static, 1000);
#if CLI_ENABLED == ENABLED
if (g.cli_enabled == 1) {
const prog_char_t *msg = PSTR("\nPress ENTER 3 times to start interactive setup\n");
cliSerial->println_P(msg);
if (gcs[1].initialised && (gcs[1].get_uart() != NULL)) {
gcs[1].get_uart()->println_P(msg);
}
if (num_gcs > 2 && gcs[2].initialised && (gcs[2].get_uart() != NULL)) {
gcs[2].get_uart()->println_P(msg);
}
}
#endif // CLI_ENABLED
startup_ground();
Log_Write_Startup(TYPE_GROUNDSTART_MSG);
// choose the nav controller
set_nav_controller();
set_mode((FlightMode)g.initial_mode.get());
// set the correct flight mode
// ---------------------------
reset_control_switch();
// initialise sensor
#if OPTFLOW == ENABLED
optflow.init();
#endif
}
// return true if a new sample is available
bool AP_InertialSensor_Oilpan::_sample_available() const
{
return apm1_adc.num_samples_available(_sensors) >= _sample_threshold;
}
void Rover::init_ardupilot()
{
// initialise console serial port
serial_manager.init_console();
cliSerial->printf_P(PSTR("\n\nInit " FIRMWARE_STRING
"\n\nFree RAM: %u\n"),
hal.util->available_memory());
//
// Check the EEPROM format version before loading any parameters from EEPROM.
//
load_parameters();
BoardConfig.init();
// initialise serial ports
serial_manager.init();
ServoRelayEvents.set_channel_mask(0xFFF0);
set_control_channels();
battery.init();
// keep a record of how many resets have happened. This can be
// used to detect in-flight resets
g.num_resets.set_and_save(g.num_resets+1);
// init baro before we start the GCS, so that the CLI baro test works
barometer.init();
// init the GCS
gcs[0].setup_uart(serial_manager, AP_SerialManager::SerialProtocol_Console, 0);
// we start by assuming USB connected, as we initialed the serial
// port with SERIAL0_BAUD. check_usb_mux() fixes this if need be.
usb_connected = true;
check_usb_mux();
// setup serial port for telem1
gcs[1].setup_uart(serial_manager, AP_SerialManager::SerialProtocol_MAVLink, 0);
#if MAVLINK_COMM_NUM_BUFFERS > 2
// setup serial port for telem2
gcs[2].setup_uart(serial_manager, AP_SerialManager::SerialProtocol_MAVLink, 1);
#endif
#if MAVLINK_COMM_NUM_BUFFERS > 3
// setup serial port for fourth telemetry port (not used by default)
gcs[3].setup_uart(serial_manager, AP_SerialManager::SerialProtocol_MAVLink, 2);
#endif
// setup frsky telemetry
#if FRSKY_TELEM_ENABLED == ENABLED
frsky_telemetry.init(serial_manager);
#endif
mavlink_system.sysid = g.sysid_this_mav;
#if LOGGING_ENABLED == ENABLED
log_init();
#endif
// Register mavlink_delay_cb, which will run anytime you have
// more than 5ms remaining in your call to hal.scheduler->delay
hal.scheduler->register_delay_callback(mavlink_delay_cb_static, 5);
#if CONFIG_HAL_BOARD == HAL_BOARD_APM1
apm1_adc.Init(); // APM ADC library initialization
#endif
if (g.compass_enabled==true) {
if (!compass.init()|| !compass.read()) {
cliSerial->println_P(PSTR("Compass initialisation failed!"));
g.compass_enabled = false;
} else {
ahrs.set_compass(&compass);
//compass.get_offsets(); // load offsets to account for airframe magnetic interference
}
}
// initialise sonar
init_sonar();
// and baro for EKF
init_barometer();
// Do GPS init
gps.init(&DataFlash, serial_manager);
rc_override_active = hal.rcin->set_overrides(rc_override, 8);
init_rc_in(); // sets up rc channels from radio
init_rc_out(); // sets up the timer libs
relay.init();
#if MOUNT == ENABLED
// initialise camera mount
camera_mount.init(serial_manager);
#endif
/*
setup the 'main loop is dead' check. Note that this relies on
the RC library being initialised.
*/
hal.scheduler->register_timer_failsafe(failsafe_check_static, 1000);
#if CLI_ENABLED == ENABLED
// If the switch is in 'menu' mode, run the main menu.
//
// Since we can't be sure that the setup or test mode won't leave
// the system in an odd state, we don't let the user exit the top
// menu; they must reset in order to fly.
//
if (g.cli_enabled == 1) {
const prog_char_t *msg = PSTR("\nPress ENTER 3 times to start interactive setup\n");
cliSerial->println_P(msg);
if (gcs[1].initialised && (gcs[1].get_uart() != NULL)) {
gcs[1].get_uart()->println_P(msg);
}
if (num_gcs > 2 && gcs[2].initialised && (gcs[2].get_uart() != NULL)) {
gcs[2].get_uart()->println_P(msg);
}
}
#endif
startup_ground();
Log_Write_Startup(TYPE_GROUNDSTART_MSG);
set_mode((enum mode)g.initial_mode.get());
// set the correct flight mode
// ---------------------------
reset_control_switch();
}
