// Prepare message for battery
void prepare_battery_status(void)
{
adc_on(true);
_delay_ms(10);
uint16_t percentage = bat_percentage(read_battery(), 1100); // 1.1V * 2 cells = 2.2V = min. voltage for RFM12B
adc_on(false);
UART_PUTF("Sending battery: %u%%\r\n", percentage);
// Set packet content
pkg_header_init_generic_batterystatus_status();
msg_generic_batterystatus_set_percentage(percentage);
}
// update_batt_compass - read battery and compass
// should be called at 10hz
void Copter::update_batt_compass(void)
{
// read battery before compass because it may be used for motor interference compensation
read_battery();
if(g.compass_enabled) {
// update compass with throttle value - used for compassmot
compass.set_throttle(motors.get_throttle()/1000.0f);
compass.read();
// log compass information
if (should_log(MASK_LOG_COMPASS)) {
DataFlash.Log_Write_Compass(compass);
}
}
}
void BatteryTestStart::runTest() {
uint32_t battery_level = 0;
QString *str;
str = new QString("Waiting for AC to be unplugged...");
emit testStateUpdated(TEST_INFO, 0, str);
#ifdef linux
/* Wait for AC to be unplugged */
while (gpio_get_value(AC_PRESENT_GPIO))
usleep(10000);
battery_level = read_battery();
#endif
str = new QString();
str->sprintf("Battery level: %d mV", battery_level);
emit testStateUpdated(TEST_INFO, 0, str);
return;
}
// setup_compassmot - sets compass's motor interference parameters
uint8_t Copter::mavlink_compassmot(mavlink_channel_t chan)
{
#if FRAME_CONFIG == HELI_FRAME
// compassmot not implemented for tradheli
return 1;
#else
int8_t comp_type; // throttle or current based compensation
Vector3f compass_base[COMPASS_MAX_INSTANCES]; // compass vector when throttle is zero
Vector3f motor_impact[COMPASS_MAX_INSTANCES]; // impact of motors on compass vector
Vector3f motor_impact_scaled[COMPASS_MAX_INSTANCES]; // impact of motors on compass vector scaled with throttle
Vector3f motor_compensation[COMPASS_MAX_INSTANCES]; // final compensation to be stored to eeprom
float throttle_pct; // throttle as a percentage 0.0 ~ 1.0
float throttle_pct_max = 0.0f; // maximum throttle reached (as a percentage 0~1.0)
float current_amps_max = 0.0f; // maximum current reached
float interference_pct[COMPASS_MAX_INSTANCES]; // interference as a percentage of total mag field (for reporting purposes only)
uint32_t last_run_time;
uint32_t last_send_time;
bool updated = false; // have we updated the compensation vector at least once
uint8_t command_ack_start = command_ack_counter;
// exit immediately if we are already in compassmot
if (ap.compass_mot) {
// ignore restart messages
return 1;
}else{
ap.compass_mot = true;
}
// initialise output
for (uint8_t i=0; i<COMPASS_MAX_INSTANCES; i++) {
interference_pct[i] = 0.0f;
}
// check compass is enabled
if (!g.compass_enabled) {
gcs[chan-MAVLINK_COMM_0].send_text(MAV_SEVERITY_CRITICAL, "Compass disabled");
ap.compass_mot = false;
return 1;
}
// check compass health
compass.read();
for (uint8_t i=0; i<compass.get_count(); i++) {
if (!compass.healthy(i)) {
gcs[chan-MAVLINK_COMM_0].send_text(MAV_SEVERITY_CRITICAL, "Check compass");
ap.compass_mot = false;
return 1;
}
}
// check if radio is calibrated
pre_arm_rc_checks();
if (!ap.pre_arm_rc_check) {
gcs[chan-MAVLINK_COMM_0].send_text(MAV_SEVERITY_CRITICAL, "RC not calibrated");
ap.compass_mot = false;
return 1;
}
// check throttle is at zero
read_radio();
if (channel_throttle->control_in != 0) {
gcs[chan-MAVLINK_COMM_0].send_text(MAV_SEVERITY_CRITICAL, "Throttle not zero");
ap.compass_mot = false;
return 1;
}
// check we are landed
if (!ap.land_complete) {
gcs[chan-MAVLINK_COMM_0].send_text(MAV_SEVERITY_CRITICAL, "Not landed");
ap.compass_mot = false;
return 1;
}
// disable cpu failsafe
failsafe_disable();
// initialise compass
init_compass();
// default compensation type to use current if possible
if (battery.has_current()) {
comp_type = AP_COMPASS_MOT_COMP_CURRENT;
}else{
comp_type = AP_COMPASS_MOT_COMP_THROTTLE;
}
// send back initial ACK
mavlink_msg_command_ack_send(chan, MAV_CMD_PREFLIGHT_CALIBRATION,0);
// flash leds
AP_Notify::flags.esc_calibration = true;
// warn user we are starting calibration
gcs[chan-MAVLINK_COMM_0].send_text(MAV_SEVERITY_INFO, "Starting calibration");
// inform what type of compensation we are attempting
if (comp_type == AP_COMPASS_MOT_COMP_CURRENT) {
gcs[chan-MAVLINK_COMM_0].send_text(MAV_SEVERITY_INFO, "Current");
} else{
gcs[chan-MAVLINK_COMM_0].send_text(MAV_SEVERITY_INFO, "Throttle");
}
// disable throttle and battery failsafe
g.failsafe_throttle = FS_THR_DISABLED;
g.failsafe_battery_enabled = FS_BATT_DISABLED;
// disable motor compensation
compass.motor_compensation_type(AP_COMPASS_MOT_COMP_DISABLED);
for (uint8_t i=0; i<compass.get_count(); i++) {
compass.set_motor_compensation(i, Vector3f(0,0,0));
}
// get initial compass readings
last_run_time = millis();
while ( millis() - last_run_time < 500 ) {
compass.accumulate();
}
compass.read();
// store initial x,y,z compass values
// initialise interference percentage
for (uint8_t i=0; i<compass.get_count(); i++) {
compass_base[i] = compass.get_field(i);
interference_pct[i] = 0.0f;
}
// enable motors and pass through throttle
init_rc_out();
enable_motor_output();
motors.armed(true);
// initialise run time
last_run_time = millis();
last_send_time = millis();
// main run while there is no user input and the compass is healthy
while (command_ack_start == command_ack_counter && compass.healthy(compass.get_primary()) && motors.armed()) {
// 50hz loop
if (millis() - last_run_time < 20) {
// grab some compass values
compass.accumulate();
hal.scheduler->delay(5);
continue;
}
last_run_time = millis();
// read radio input
read_radio();
// pass through throttle to motors
motors.throttle_pass_through(channel_throttle->radio_in);
// read some compass values
compass.read();
// read current
read_battery();
// calculate scaling for throttle
throttle_pct = (float)channel_throttle->control_in / 1000.0f;
throttle_pct = constrain_float(throttle_pct,0.0f,1.0f);
// if throttle is near zero, update base x,y,z values
if (throttle_pct <= 0.0f) {
for (uint8_t i=0; i<compass.get_count(); i++) {
compass_base[i] = compass_base[i] * 0.99f + compass.get_field(i) * 0.01f;
}
// causing printing to happen as soon as throttle is lifted
} else {
// calculate diff from compass base and scale with throttle
for (uint8_t i=0; i<compass.get_count(); i++) {
motor_impact[i] = compass.get_field(i) - compass_base[i];
}
// throttle based compensation
if (comp_type == AP_COMPASS_MOT_COMP_THROTTLE) {
// for each compass
for (uint8_t i=0; i<compass.get_count(); i++) {
// scale by throttle
motor_impact_scaled[i] = motor_impact[i] / throttle_pct;
// adjust the motor compensation to negate the impact
motor_compensation[i] = motor_compensation[i] * 0.99f - motor_impact_scaled[i] * 0.01f;
}
updated = true;
} else {
// for each compass
for (uint8_t i=0; i<compass.get_count(); i++) {
// current based compensation if more than 3amps being drawn
motor_impact_scaled[i] = motor_impact[i] / battery.current_amps();
// adjust the motor compensation to negate the impact if drawing over 3amps
if (battery.current_amps() >= 3.0f) {
motor_compensation[i] = motor_compensation[i] * 0.99f - motor_impact_scaled[i] * 0.01f;
updated = true;
}
}
}
// calculate interference percentage at full throttle as % of total mag field
if (comp_type == AP_COMPASS_MOT_COMP_THROTTLE) {
for (uint8_t i=0; i<compass.get_count(); i++) {
// interference is impact@fullthrottle / mag field * 100
interference_pct[i] = motor_compensation[i].length() / (float)COMPASS_MAGFIELD_EXPECTED * 100.0f;
}
}else{
for (uint8_t i=0; i<compass.get_count(); i++) {
// interference is impact/amp * (max current seen / max throttle seen) / mag field * 100
interference_pct[i] = motor_compensation[i].length() * (current_amps_max/throttle_pct_max) / (float)COMPASS_MAGFIELD_EXPECTED * 100.0f;
}
}
// record maximum throttle and current
throttle_pct_max = MAX(throttle_pct_max, throttle_pct);
current_amps_max = MAX(current_amps_max, battery.current_amps());
}
if (AP_HAL::millis() - last_send_time > 500) {
last_send_time = AP_HAL::millis();
mavlink_msg_compassmot_status_send(chan,
channel_throttle->control_in,
battery.current_amps(),
interference_pct[compass.get_primary()],
motor_compensation[compass.get_primary()].x,
motor_compensation[compass.get_primary()].y,
motor_compensation[compass.get_primary()].z);
}
}
// stop motors
motors.output_min();
motors.armed(false);
// set and save motor compensation
if (updated) {
compass.motor_compensation_type(comp_type);
for (uint8_t i=0; i<compass.get_count(); i++) {
compass.set_motor_compensation(i, motor_compensation[i]);
}
compass.save_motor_compensation();
// display success message
gcs[chan-MAVLINK_COMM_0].send_text(MAV_SEVERITY_INFO, "Calibration successful");
} else {
// compensation vector never updated, report failure
gcs[chan-MAVLINK_COMM_0].send_text(MAV_SEVERITY_NOTICE, "Failed");
compass.motor_compensation_type(AP_COMPASS_MOT_COMP_DISABLED);
}
// display new motor offsets and save
report_compass();
// turn off notify leds
AP_Notify::flags.esc_calibration = false;
// re-enable cpu failsafe
failsafe_enable();
// re-enable failsafes
g.failsafe_throttle.load();
g.failsafe_battery_enabled.load();
// flag we have completed
ap.compass_mot = false;
return 0;
#endif // FRAME_CONFIG != HELI_FRAME
}
// Initialise board
void board_init (void) {
#ifndef SIMULATE
io_init(); // Init GPIOs
uart_init(BAUD_RATE);
stderr = &uartio;
printf(str_boot_uart,BAUD_RATE);
printf(str_boot_start);
#else
printf("Skipping UART initialization...\n");
#endif
#ifndef SIMULATE
digital_init();
#endif
encoder_init();
#ifndef SIMULATE
spi_init();
motor_init();
servo_init();
#ifdef LCD_DEBUG
lcd_init(); //consider wrapping this in an #ifdef LCD_DEBUG tag?
stdout = &lcdout;
#else
stdout = &uartio;
stdin = &uartio;
#endif
adc_init();
isr_init();
memory_init();
#endif
// load config, or fail if invalid
if (!board_load_config())
board_fail("Bad Config");
printf(str_boot_conf);
printf(str_boot_board,
board_config.version>>8,
board_config.version&0xFF);
printf(str_boot_id, board_config.id);
// print boot text to screen
printf(str_boot_message, board_config.version>>8, board_config.version&0xFF);
// check battery, fail if <7.5V
printf(str_boot_batt,read_battery());
#ifdef CHECK_BATTERY
if (!(read_battery()>=7200)) {
// NOTE: in the current 2-battery version of the HappyBoard, the
// battery voltage is the motor battery (P+). Holding GO overrides
// the check so you can run the HappyBoard without a motor battery.
if (go_press())
printf("WARNING: LOW BATTERY\n");
else
board_fail("Low battery");
} else {
printf("Battery OK\n");
}
#endif
#ifndef SIMULATE
// initialise FPGA
if (!fpga_init(FPGA_CONFIG_ADDRESS, board_config.fpga_len))
board_fail("FPGA failure");
printf(str_boot_fpga, fpga_get_version_major(), fpga_get_version_minor());
#else
printf("Skipping FPGA initialization...\n");
#endif
// all ok
#ifndef SIMULATE
#ifdef LCD_DEBUG
lcd_set_pos(31);
lcd_print_char('\1', NULL);
#else
printf("Board init complete.\n");
#endif
#else
printf("Board init complete.\n");
#endif
#ifndef SIMULATE
LED_COMM(0);
#endif
}
