void mavlink_common_message_handler(const mavlink_message_t *msg) { switch (msg->msgid) { case MAVLINK_MSG_ID_HEARTBEAT: break; /* When requesting data streams say we can't send them */ case MAVLINK_MSG_ID_REQUEST_DATA_STREAM: { mavlink_request_data_stream_t cmd; mavlink_msg_request_data_stream_decode(msg, &cmd); mavlink_msg_data_stream_send(MAVLINK_COMM_0, cmd.req_stream_id, 0, 0); MAVLinkSendMessage(); break; } /* Override channels with RC */ case MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE: { mavlink_rc_channels_override_t cmd; mavlink_msg_rc_channels_override_decode(msg, &cmd); #if defined RADIO_CONTROL && defined RADIO_CONTROL_TYPE_DATALINK uint8_t thrust = (cmd.chan3_raw - 950) * 127 / 1100; int8_t roll = -(cmd.chan1_raw - 1500) * 255 / 1100 / 2; int8_t pitch = -(cmd.chan2_raw - 1500) * 255 / 1100 / 2; int8_t yaw = -(cmd.chan4_raw - 1500) * 255 / 1100; parse_rc_4ch_datalink(0, thrust, roll, pitch, yaw); //printf("RECEIVED: RC Channel Override for: %d/%d: throttle: %d; roll: %d; pitch: %d; yaw: %d;\r\n", // cmd.target_system, cmd.target_component, thrust, roll, pitch, yaw); #endif break; } /* When a request is made of the parameters list */ case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: { mavlink_params_idx = 0; break; } /* When a request os made for a specific parameter */ case MAVLINK_MSG_ID_PARAM_REQUEST_READ: { mavlink_param_request_read_t cmd; mavlink_msg_param_request_read_decode(msg, &cmd); // First check param_index and search for the ID if needed if (cmd.param_index == -1) { cmd.param_index = settings_idx_from_param_id(cmd.param_id); } // Send message only if the param_index was found (Coverity Scan) if (cmd.param_index > -1) { mavlink_msg_param_value_send(MAVLINK_COMM_0, mavlink_param_names[cmd.param_index], settings_get_value(cmd.param_index), MAV_PARAM_TYPE_REAL32, NB_SETTING, cmd.param_index); MAVLinkSendMessage(); } break; } case MAVLINK_MSG_ID_PARAM_SET: { mavlink_param_set_t set; mavlink_msg_param_set_decode(msg, &set); // Check if this message is for this system if ((uint8_t) set.target_system == AC_ID) { int16_t idx = settings_idx_from_param_id(set.param_id); // setting found if (idx >= 0) { // Only write if new value is NOT "not-a-number" // AND is NOT infinity if (set.param_type == MAV_PARAM_TYPE_REAL32 && !isnan(set.param_value) && !isinf(set.param_value)) { DlSetting(idx, set.param_value); // Report back new value mavlink_msg_param_value_send(MAVLINK_COMM_0, mavlink_param_names[idx], settings_get_value(idx), MAV_PARAM_TYPE_REAL32, NB_SETTING, idx); MAVLinkSendMessage(); } } } } break; #ifndef AP /* only for rotorcraft */ case MAVLINK_MSG_ID_COMMAND_LONG: { mavlink_command_long_t cmd; mavlink_msg_command_long_decode(msg, &cmd); // Check if this message is for this system if ((uint8_t) cmd.target_system == AC_ID) { uint8_t result = MAV_RESULT_UNSUPPORTED; switch (cmd.command) { case MAV_CMD_NAV_GUIDED_ENABLE: MAVLINK_DEBUG("got cmd NAV_GUIDED_ENABLE: %f\n", cmd.param1); result = MAV_RESULT_FAILED; if (cmd.param1 > 0.5) { autopilot_set_mode(AP_MODE_GUIDED); if (autopilot_mode == AP_MODE_GUIDED) { result = MAV_RESULT_ACCEPTED; } } else { // turn guided mode off - to what? maybe NAV? or MODE_AUTO2? } break; case MAV_CMD_COMPONENT_ARM_DISARM: /* supposed to use this command to arm or SET_MODE?? */ MAVLINK_DEBUG("got cmd COMPONENT_ARM_DISARM: %f\n", cmd.param1); result = MAV_RESULT_FAILED; if (cmd.param1 > 0.5) { autopilot_set_motors_on(TRUE); if (autopilot_motors_on) result = MAV_RESULT_ACCEPTED; } else { autopilot_set_motors_on(FALSE); if (!autopilot_motors_on) result = MAV_RESULT_ACCEPTED; } break; default: break; } // confirm command with result mavlink_msg_command_ack_send(MAVLINK_COMM_0, cmd.command, result); MAVLinkSendMessage(); } break; } case MAVLINK_MSG_ID_SET_MODE: { mavlink_set_mode_t mode; mavlink_msg_set_mode_decode(msg, &mode); if (mode.target_system == AC_ID) { MAVLINK_DEBUG("got SET_MODE: base_mode:%d\n", mode.base_mode); if (mode.base_mode & MAV_MODE_FLAG_SAFETY_ARMED) { autopilot_set_motors_on(TRUE); } else { autopilot_set_motors_on(FALSE); } if (mode.base_mode & MAV_MODE_FLAG_GUIDED_ENABLED) { autopilot_set_mode(AP_MODE_GUIDED); } else if (mode.base_mode & MAV_MODE_FLAG_AUTO_ENABLED) { autopilot_set_mode(AP_MODE_NAV); } } break; } case MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED: { mavlink_set_position_target_local_ned_t target; mavlink_msg_set_position_target_local_ned_decode(msg, &target); // Check if this message is for this system if (target.target_system == AC_ID) { MAVLINK_DEBUG("SET_POSITION_TARGET_LOCAL_NED, byte_mask: %d\n", target.type_mask); /* if position and yaw bits are not set to ignored, use only position for now */ if (!(target.type_mask & 0b1110000000100000)) { switch (target.coordinate_frame) { case MAV_FRAME_LOCAL_NED: MAVLINK_DEBUG("set position target, frame LOCAL_NED\n"); autopilot_guided_goto_ned(target.x, target.y, target.z, target.yaw); break; case MAV_FRAME_LOCAL_OFFSET_NED: MAVLINK_DEBUG("set position target, frame LOCAL_OFFSET_NED\n"); autopilot_guided_goto_ned_relative(target.x, target.y, target.z, target.yaw); break; case MAV_FRAME_BODY_OFFSET_NED: MAVLINK_DEBUG("set position target, frame BODY_OFFSET_NED\n"); autopilot_guided_goto_body_relative(target.x, target.y, target.z, target.yaw); break; default: break; } } else if (!(target.type_mask & 0b0001110000100000)) { /* position is set to ignore, but velocity not */ switch (target.coordinate_frame) { case MAV_FRAME_LOCAL_NED: MAVLINK_DEBUG("set velocity target, frame LOCAL_NED\n"); autopilot_guided_move_ned(target.vx, target.vy, target.vz, target.yaw); break; default: break; } } } break; } #endif default: //Do nothing MAVLINK_DEBUG("Received message with id: %d\r\n", msg->msgid); break; } }
void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) { switch (msg->msgid) { // If we are currently operating as a proxy for a remote, // alas we have to look inside each packet to see if its for us or for the remote case MAVLINK_MSG_ID_REQUEST_DATA_STREAM: { handle_request_data_stream(msg, true); break; } case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: { handle_param_request_list(msg); break; } case MAVLINK_MSG_ID_PARAM_REQUEST_READ: { handle_param_request_read(msg); break; } case MAVLINK_MSG_ID_PARAM_SET: { handle_param_set(msg, NULL); break; } case MAVLINK_MSG_ID_HEARTBEAT: break; case MAVLINK_MSG_ID_COMMAND_LONG: { // decode mavlink_command_long_t packet; mavlink_msg_command_long_decode(msg, &packet); uint8_t result = MAV_RESULT_UNSUPPORTED; // do command send_text_P(SEVERITY_LOW,PSTR("command received: ")); switch(packet.command) { case MAV_CMD_PREFLIGHT_CALIBRATION: { if (is_equal(packet.param1,1.0f)) { tracker.ins.init_gyro(); if (tracker.ins.gyro_calibrated_ok_all()) { tracker.ahrs.reset_gyro_drift(); result = MAV_RESULT_ACCEPTED; } else { result = MAV_RESULT_FAILED; } } if (is_equal(packet.param3,1.0f)) { tracker.init_barometer(); // zero the altitude difference on next baro update tracker.nav_status.need_altitude_calibration = true; } if (is_equal(packet.param4,1.0f)) { // Cant trim radio } else if (is_equal(packet.param5,1.0f)) { float trim_roll, trim_pitch; AP_InertialSensor_UserInteract_MAVLink interact(this); if(tracker.ins.calibrate_accel(&interact, trim_roll, trim_pitch)) { // reset ahrs's trim to suggested values from calibration routine tracker.ahrs.set_trim(Vector3f(trim_roll, trim_pitch, 0)); } } else if (is_equal(packet.param5,2.0f)) { // accel trim float trim_roll, trim_pitch; if(tracker.ins.calibrate_trim(trim_roll, trim_pitch)) { // reset ahrs's trim to suggested values from calibration routine tracker.ahrs.set_trim(Vector3f(trim_roll, trim_pitch, 0)); result = MAV_RESULT_ACCEPTED; } else { result = MAV_RESULT_FAILED; } } result = MAV_RESULT_ACCEPTED; break; } case MAV_CMD_COMPONENT_ARM_DISARM: if (packet.target_component == MAV_COMP_ID_SYSTEM_CONTROL) { if (is_equal(packet.param1,1.0f)) { tracker.arm_servos(); result = MAV_RESULT_ACCEPTED; } else if (is_zero(packet.param1)) { tracker.disarm_servos(); result = MAV_RESULT_ACCEPTED; } else { result = MAV_RESULT_UNSUPPORTED; } } else { result = MAV_RESULT_UNSUPPORTED; } break; case MAV_CMD_DO_SET_MODE: switch ((uint16_t)packet.param1) { case MAV_MODE_MANUAL_ARMED: case MAV_MODE_MANUAL_DISARMED: tracker.set_mode(MANUAL); result = MAV_RESULT_ACCEPTED; break; case MAV_MODE_AUTO_ARMED: case MAV_MODE_AUTO_DISARMED: tracker.set_mode(AUTO); result = MAV_RESULT_ACCEPTED; break; default: result = MAV_RESULT_UNSUPPORTED; } break; case MAV_CMD_DO_SET_SERVO: if (tracker.servo_test_set_servo(packet.param1, packet.param2)) { result = MAV_RESULT_ACCEPTED; } break; // mavproxy/mavutil sends this when auto command is entered case MAV_CMD_MISSION_START: tracker.set_mode(AUTO); result = MAV_RESULT_ACCEPTED; break; case MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN: { if (is_equal(packet.param1,1.0f) || is_equal(packet.param1,3.0f)) { // when packet.param1 == 3 we reboot to hold in bootloader hal.scheduler->reboot(is_equal(packet.param1,3.0f)); result = MAV_RESULT_ACCEPTED; } break; } case MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES: { if (is_equal(packet.param1,1.0f)) { tracker.gcs[chan-MAVLINK_COMM_0].send_autopilot_version(); result = MAV_RESULT_ACCEPTED; } break; } default: break; } mavlink_msg_command_ack_send( chan, packet.command, result); break; } // When mavproxy 'wp sethome' case MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST: { // decode mavlink_mission_write_partial_list_t packet; mavlink_msg_mission_write_partial_list_decode(msg, &packet); if (packet.start_index == 0) { // New home at wp index 0. Ask for it waypoint_receiving = true; waypoint_request_i = 0; waypoint_request_last = 0; send_message(MSG_NEXT_WAYPOINT); waypoint_receiving = true; } break; } // XXX receive a WP from GCS and store in EEPROM if it is HOME case MAVLINK_MSG_ID_MISSION_ITEM: { // decode mavlink_mission_item_t packet; uint8_t result = MAV_MISSION_ACCEPTED; mavlink_msg_mission_item_decode(msg, &packet); struct Location tell_command = {}; switch (packet.frame) { case MAV_FRAME_MISSION: case MAV_FRAME_GLOBAL: { tell_command.lat = 1.0e7f*packet.x; // in as DD converted to * t7 tell_command.lng = 1.0e7f*packet.y; // in as DD converted to * t7 tell_command.alt = packet.z*1.0e2f; // in as m converted to cm tell_command.options = 0; // absolute altitude break; } #ifdef MAV_FRAME_LOCAL_NED case MAV_FRAME_LOCAL_NED: // local (relative to home position) { tell_command.lat = 1.0e7f*ToDeg(packet.x/ (RADIUS_OF_EARTH*cosf(ToRad(home.lat/1.0e7f)))) + home.lat; tell_command.lng = 1.0e7f*ToDeg(packet.y/RADIUS_OF_EARTH) + home.lng; tell_command.alt = -packet.z*1.0e2f; tell_command.options = MASK_OPTIONS_RELATIVE_ALT; break; } #endif #ifdef MAV_FRAME_LOCAL case MAV_FRAME_LOCAL: // local (relative to home position) { tell_command.lat = 1.0e7f*ToDeg(packet.x/ (RADIUS_OF_EARTH*cosf(ToRad(home.lat/1.0e7f)))) + home.lat; tell_command.lng = 1.0e7f*ToDeg(packet.y/RADIUS_OF_EARTH) + home.lng; tell_command.alt = packet.z*1.0e2f; tell_command.options = MASK_OPTIONS_RELATIVE_ALT; break; } #endif case MAV_FRAME_GLOBAL_RELATIVE_ALT: // absolute lat/lng, relative altitude { tell_command.lat = 1.0e7f * packet.x; // in as DD converted to * t7 tell_command.lng = 1.0e7f * packet.y; // in as DD converted to * t7 tell_command.alt = packet.z * 1.0e2f; tell_command.options = MASK_OPTIONS_RELATIVE_ALT; // store altitude relative!! Always!! break; } default: result = MAV_MISSION_UNSUPPORTED_FRAME; break; } if (result != MAV_MISSION_ACCEPTED) goto mission_failed; // Check if receiving waypoints (mission upload expected) if (!waypoint_receiving) { result = MAV_MISSION_ERROR; goto mission_failed; } // check if this is the HOME wp if (packet.seq == 0) { tracker.set_home(tell_command); // New home in EEPROM send_text_P(SEVERITY_LOW,PSTR("new HOME received")); waypoint_receiving = false; } mission_failed: // we are rejecting the mission/waypoint mavlink_msg_mission_ack_send( chan, msg->sysid, msg->compid, result); break; } case MAVLINK_MSG_ID_MANUAL_CONTROL: { mavlink_manual_control_t packet; mavlink_msg_manual_control_decode(msg, &packet); tracker.tracking_manual_control(packet); break; } case MAVLINK_MSG_ID_GLOBAL_POSITION_INT: { // decode mavlink_global_position_int_t packet; mavlink_msg_global_position_int_decode(msg, &packet); tracker.tracking_update_position(packet); break; } case MAVLINK_MSG_ID_SCALED_PRESSURE: { // decode mavlink_scaled_pressure_t packet; mavlink_msg_scaled_pressure_decode(msg, &packet); tracker.tracking_update_pressure(packet); break; } case MAVLINK_MSG_ID_SET_MODE: { handle_set_mode(msg, FUNCTOR_BIND(&tracker, &Tracker::mavlink_set_mode, bool, uint8_t)); break; } #if HAL_CPU_CLASS > HAL_CPU_CLASS_16 case MAVLINK_MSG_ID_SERIAL_CONTROL: handle_serial_control(msg, tracker.gps); break; case MAVLINK_MSG_ID_GPS_INJECT_DATA: handle_gps_inject(msg, tracker.gps); break; #endif case MAVLINK_MSG_ID_AUTOPILOT_VERSION_REQUEST: tracker.gcs[chan-MAVLINK_COMM_0].send_autopilot_version(); break; } // end switch } // end handle mavlink
/**************************************************************************** * Public Functions ****************************************************************************/ void handleMessage(mavlink_message_t * msg) { //check for terminate command if(msg->msgid == MAVLINK_MSG_ID_COMMAND_LONG) { mavlink_command_long_t cmd; mavlink_msg_command_long_decode(msg, &cmd); if (cmd.target_system == mavlink_system.sysid && ((cmd.target_component == mavlink_system.compid) || (cmd.target_component == MAV_COMP_ID_ALL))) { bool terminate_link = false; bool reboot = false; /* result of the command */ uint8_t result = MAV_RESULT_UNSUPPORTED; /* supported command handling start */ /* request to set different system mode */ if (cmd.command == MAV_CMD_DO_SET_MODE) { mavlink_system.mode = cmd.param1; } /* request to arm */ // XXX /* request for an autopilot reboot */ if (cmd.command == MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN && cmd.param1 == 1.0f) { reboot = true; result = MAV_RESULT_ACCEPTED; mavlink_msg_statustext_send(chan,0,"Rebooting autopilot.. "); } /* request for a link shutdown */ if (cmd.command == MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN && cmd.param1 == 3.0f) { terminate_link = true; result = MAV_RESULT_ACCEPTED; mavlink_msg_statustext_send(chan,0,"Terminating MAVLink.. "); } /* supported command handling stop */ /* send any requested ACKs */ if (cmd.confirmation > 0) { mavlink_msg_command_ack_send(chan, cmd.command, result); } /* the two termination / reset commands need special handling */ if (terminate_link) { printf("MAVLink: Terminating.. \n"); fflush(stdout); /* sleep 100 ms to allow UART buffer to empty */ led_off(LED_BLUE); led_on(LED_AMBER); int i; for (i = 0; i < 5; i++) { led_toggle(LED_AMBER); usleep(20000); } //terminate other threads: pthread_cancel(heartbeat_thread); //terminate this thread (receive_thread) pthread_exit(NULL); } if (reboot) { printf("MAVLink: Rebooting system.. \n"); fflush(stdout); /* sleep 100 ms to allow UART buffer to empty */ led_off(LED_BLUE); led_on(LED_AMBER); int i; for (i = 0; i < 5; i++) { led_toggle(LED_BLUE); led_toggle(LED_AMBER); usleep(20000); } //terminate other threads: pthread_cancel(heartbeat_thread); // Reset whole system /* Resetting CPU */ // FIXME Need check for ARM architecture here #ifndef NVIC_AIRCR #define NVIC_AIRCR (*((uint32_t*)0xE000ED0C)) #endif /* Set the SYSRESETREQ bit to force a reset */ NVIC_AIRCR = 0x05fa0004; /* Spinning until the board is really reset */ while(true); } } } /* Handle quadrotor motor setpoints */ if(msg->msgid == MAVLINK_MSG_ID_SET_QUAD_MOTORS_SETPOINT) { mavlink_set_quad_motors_setpoint_t quad_motors_setpoint; mavlink_msg_set_quad_motors_setpoint_decode(msg, &quad_motors_setpoint); if(quad_motors_setpoint.target_system == mavlink_system.sysid) { global_data_lock(&global_data_quad_motors_setpoint.access_conf); global_data_quad_motors_setpoint.motor_front_nw = quad_motors_setpoint.motor_front_nw; global_data_quad_motors_setpoint.motor_right_ne = quad_motors_setpoint.motor_right_ne; global_data_quad_motors_setpoint.motor_back_se = quad_motors_setpoint.motor_back_se; global_data_quad_motors_setpoint.motor_left_sw = quad_motors_setpoint.motor_left_sw; global_data_quad_motors_setpoint.counter++; global_data_quad_motors_setpoint.timestamp = global_data_get_timestamp_milliseconds(); global_data_unlock(&global_data_quad_motors_setpoint.access_conf); /* Inform the other processes that there is new gps data available */ global_data_broadcast(&global_data_quad_motors_setpoint.access_conf); } } }
// 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 }
void handle_mavlink_message(mavlink_channel_t chan, mavlink_message_t* msg) { /* all messages from usart2 are directly forwarded */ if(chan == MAVLINK_COMM_1) { uint8_t buf[MAVLINK_MAX_PACKET_LEN]; uint32_t len; // Copy to USART 3 len = mavlink_msg_to_send_buffer(buf, msg); mavlink_send_uart_bytes(MAVLINK_COMM_0, buf, len); if(global_data.param[PARAM_USB_SEND_FORWARD]) mavlink_send_uart_bytes(MAVLINK_COMM_2, buf, len); return; } /* forwarded received messages from usb and usart3 to usart2 */ uint8_t buf[MAVLINK_MAX_PACKET_LEN]; uint32_t len; /* copy to usart2 */ len = mavlink_msg_to_send_buffer(buf, msg); for (int i = 0; i < len; i++) { usart2_tx_ringbuffer_push(buf, len); } /* handling messages */ switch (msg->msgid) { case MAVLINK_MSG_ID_PARAM_REQUEST_READ: { mavlink_param_request_read_t set; mavlink_msg_param_request_read_decode(msg, &set); /* Check if this message is for this system */ if ((uint8_t) set.target_system == (uint8_t) global_data.param[PARAM_SYSTEM_ID] && (uint8_t) set.target_component == (uint8_t) global_data.param[PARAM_COMPONENT_ID]) { char* key = (char*) set.param_id; if (set.param_id[0] != -1) { /* Choose parameter based on index */ if ((set.param_index >= 0) && (set.param_index < ONBOARD_PARAM_COUNT)) { /* Report back value */ mavlink_msg_param_value_send(chan, global_data.param_name[set.param_index], global_data.param[set.param_index], MAVLINK_TYPE_FLOAT, ONBOARD_PARAM_COUNT, set.param_index); } } else { for (int i = 0; i < ONBOARD_PARAM_COUNT; i++) { bool match = true; for (int j = 0; j < ONBOARD_PARAM_NAME_LENGTH; j++) { /* Compare */ if (((char) (global_data.param_name[i][j])) != (char) (key[j])) { match = false; } /* End matching if null termination is reached */ if (((char) global_data.param_name[i][j]) == '\0') { break; } } /* Check if matched */ if (match) { /* Report back value */ mavlink_msg_param_value_send(chan, global_data.param_name[i], global_data.param[i], MAVLINK_TYPE_FLOAT, ONBOARD_PARAM_COUNT, i); } } } } } break; case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: { /* Start sending parameters */ m_parameter_i = 0; } break; case MAVLINK_MSG_ID_PARAM_SET: { mavlink_param_set_t set; mavlink_msg_param_set_decode(msg, &set); /* Check if this message is for this system */ if ((uint8_t) set.target_system == (uint8_t) global_data.param[PARAM_SYSTEM_ID] && (uint8_t) set.target_component == (uint8_t) global_data.param[PARAM_COMPONENT_ID]) { char* key = (char*) set.param_id; for (int i = 0; i < ONBOARD_PARAM_COUNT; i++) { bool match = true; for (int j = 0; j < ONBOARD_PARAM_NAME_LENGTH; j++) { /* Compare */ if (((char) (global_data.param_name[i][j])) != (char) (key[j])) { match = false; } /* End matching if null termination is reached */ if (((char) global_data.param_name[i][j]) == '\0') { break; } } /* Check if matched */ if (match) { /* Only write and emit changes if there is actually a difference * AND only write if new value is NOT "not-a-number" * AND is NOT infinity */ if (global_data.param[i] != set.param_value && !isnan(set.param_value) && !isinf(set.param_value) && global_data.param_access[i]) { global_data.param[i] = set.param_value; /* handle sensor position */ if(i == PARAM_SENSOR_POSITION) { set_sensor_position_settings((uint8_t) set.param_value); mt9v034_context_configuration(); dma_reconfigure(); buffer_reset(); } /* handle low light mode and noise correction */ else if(i == PARAM_IMAGE_LOW_LIGHT || i == PARAM_IMAGE_ROW_NOISE_CORR|| i == PARAM_IMAGE_TEST_PATTERN) { mt9v034_context_configuration(); dma_reconfigure(); buffer_reset(); } /* handle calibration on/off */ else if(i == PARAM_VIDEO_ONLY) { mt9v034_set_context(); dma_reconfigure(); buffer_reset(); if(global_data.param[PARAM_VIDEO_ONLY]) debug_string_message_buffer("Calibration Mode On"); else debug_string_message_buffer("Calibration Mode Off"); } /* handle sensor position */ else if(i == PARAM_GYRO_SENSITIVITY_DPS) { l3gd20_config(); } else { debug_int_message_buffer("Parameter received, param id =", i); } /* report back new value */ mavlink_msg_param_value_send(MAVLINK_COMM_0, global_data.param_name[i], global_data.param[i], MAVLINK_TYPE_FLOAT, ONBOARD_PARAM_COUNT, i); mavlink_msg_param_value_send(MAVLINK_COMM_2, global_data.param_name[i], global_data.param[i], MAVLINK_TYPE_FLOAT, ONBOARD_PARAM_COUNT, i); } else { /* send back current value because it is not accepted or not write access*/ mavlink_msg_param_value_send(MAVLINK_COMM_0, global_data.param_name[i], global_data.param[i], MAVLINK_TYPE_FLOAT, ONBOARD_PARAM_COUNT, i); mavlink_msg_param_value_send(MAVLINK_COMM_2, global_data.param_name[i], global_data.param[i], MAVLINK_TYPE_FLOAT, ONBOARD_PARAM_COUNT, i); } } } } } break; case MAVLINK_MSG_ID_PING: { mavlink_ping_t ping; mavlink_msg_ping_decode(msg, &ping); if (ping.target_system == 0 && ping.target_component == 0) { /* Respond to ping */ uint64_t r_timestamp = get_boot_time_us(); mavlink_msg_ping_send(chan, ping.seq, msg->sysid, msg->compid, r_timestamp); } } break; case MAVLINK_MSG_ID_COMMAND_LONG: { mavlink_command_long_t cmd; mavlink_msg_command_long_decode(msg, &cmd); switch (cmd.command) { case MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN: if (((int)(cmd.param1)) == 1) { mavlink_msg_command_ack_send(chan, cmd.command, MAV_RESULT_ACCEPTED); /* reboot */ systemreset(false); } else if (((int)(cmd.param1)) == 3) { mavlink_msg_command_ack_send(chan, cmd.command, MAV_RESULT_ACCEPTED); /* reboot to bootloader */ systemreset(true); } else { /* parameters are wrong */ mavlink_msg_command_ack_send(chan, cmd.command, MAV_RESULT_FAILED); // XXX add INVALID INPUT to MAV_RESULT } break; default: mavlink_msg_command_ack_send(chan, cmd.command, MAV_RESULT_UNSUPPORTED); break; } } break; default: break; } }