Scheduler::Scheduler(const Scheduler::conf_t config) : schedule_strategy_(config.schedule_strategy), debug_(config.debug), task_count_(0), current_schedule_slot_(0) { // allocate memory for tasks for(max_task_count_ = config.max_task_count; max_task_count_ > 0; max_task_count_--) { tasks_ = (Scheduler_task*)malloc(sizeof(Scheduler_task)*max_task_count_); if(tasks_ != NULL) { break; } } if(max_task_count_< config.max_task_count) { print_util_dbg_print("[Scheduler] constructor: tried to allocate task list for "); print_util_dbg_print_num(config.max_task_count,10); print_util_dbg_print(" tasks; only space for "); print_util_dbg_print_num(max_task_count_,10); print_util_dbg_print("\r\n"); } }
static mav_result_t state_telemetry_toggle_remote_use(state_t* state, mavlink_command_long_t* packet) { mav_result_t result = MAV_RESULT_UNSUPPORTED; if ( packet->param1 == 1) { state->remote_active = 1; state->use_mode_from_remote = 1; print_util_dbg_print("Remote control activated\r\n"); result = MAV_RESULT_ACCEPTED; } else if (packet->param1 == 0) { state->remote_active = 0; state->use_mode_from_remote = 0; print_util_dbg_print("Remote control disactivated\r\n"); result = MAV_RESULT_ACCEPTED; } return result; }
void onboard_parameters_preflight_storage(onboard_parameters_t* onboard_parameters, mavlink_command_long_t* msg) { // Onboard parameters storage if (msg->param1 == 0) { // read parameters from flash print_util_dbg_print("Reading from flashc...\r\n"); if(onboard_parameters_read_parameters_from_flashc(onboard_parameters)) { // TODO: update simulation calibration values //simulation_calib_set(&sim_model); } } else if (msg->param1 == 1) { // write parameters to flash //print_util_dbg_print("No Writing to flashc\n"); print_util_dbg_print("Writing to flashc\r\n"); onboard_parameters_write_parameters_to_flashc(onboard_parameters); } mavlink_message_t ack_msg; mavlink_msg_command_ack_pack( onboard_parameters->mavlink_stream->sysid, onboard_parameters->mavlink_stream->compid, &ack_msg, MAV_CMD_PREFLIGHT_STORAGE, MAV_RESULT_ACCEPTED ); mavlink_stream_send(onboard_parameters->mavlink_stream, &ack_msg); }
mav_result_t onboard_parameters_preflight_storage(onboard_parameters_t* onboard_parameters, mavlink_command_long_t* msg) { mav_result_t result = MAV_RESULT_DENIED; // Onboard parameters storage if (msg->param1 == 0) { // read parameters from flash print_util_dbg_print("Reading from flashc...\r\n"); if(onboard_parameters_read_parameters_from_flashc(onboard_parameters)) { result = MAV_RESULT_ACCEPTED; // TODO: update simulation calibration values //simulation_calib_set(&sim_model); } else { result = MAV_RESULT_DENIED; } } else if (msg->param1 == 1) { // write parameters to flash //print_util_dbg_print("No Writing to flashc\n"); print_util_dbg_print("Writing to flashc\r\n"); onboard_parameters_write_parameters_to_flashc(onboard_parameters); result = MAV_RESULT_ACCEPTED; } return result; }
DSTATUS disk_status (BYTE pdrv) { DSTATUS stat = STA_NOINIT; int result = RES_ERROR; if ((actual_status & STA_NOINIT)||(actual_status & STA_NODISK)) { return STA_NOINIT; } // Only MMC supported if (pdrv!=0) { return RES_PARERR; } uint8_t drive_num = MMC; switch (drive_num) { case ATA : //result = ATA_disk_status(); print_util_dbg_print("NO SUPPORTED! ATA status!\r\n"); // translate the result code here result = RES_ERROR; stat = STA_NODISK; break; case MMC : //result = MMC_disk_status(); // translate the result code here result = sd_spi_status(); if (result) { stat = 0; } else { stat = STA_NODISK; } break; case USB : //result = USB_disk_status(); print_util_dbg_print("NO SUPPORTED! USB status!\r\n"); // translate the result code here result = RES_ERROR; stat = STA_NODISK; break; } return stat; }
void onboard_parameters_add_parameter_float(onboard_parameters_t* onboard_parameters, float* val, const char* param_name) { onboard_parameters_set_t* param_set = onboard_parameters->param_set; if( val == NULL ) { print_util_dbg_print("[ONBOARD PARAMETER] Error: Null pointer!"); } else { if( param_set->param_count < param_set->max_param_count ) { onboard_parameters_entry_t* new_param = ¶m_set->parameters[param_set->param_count]; new_param->param = val; strcpy( new_param->param_name, param_name ); new_param->data_type = MAV_PARAM_TYPE_REAL32; new_param->param_name_length = strlen(param_name); new_param->schedule_for_transmission = true; param_set->param_count += 1; } else { print_util_dbg_print("[ONBOARD PARAMETER] Error: Cannot add more param\r\n"); } } }
void data_logging_add_parameter_float(data_logging_t* data_logging, float* val, const char* param_name) { data_logging_set_t* data_logging_set = data_logging->data_logging_set; if( val == NULL ) { print_util_dbg_print("[DATA LOGGING] Error: Null pointer!"); } else { if( data_logging_set->data_logging_count < data_logging_set->max_data_logging_count ) { data_logging_entry_t* new_param = &data_logging_set->data_log[data_logging_set->data_logging_count]; new_param->param = (double*) val; strcpy( new_param->param_name, param_name ); new_param->data_type = MAV_PARAM_TYPE_REAL32; data_logging_set->data_logging_count += 1; } else { print_util_dbg_print("[DATA LOGGING] Error: Cannot add more logging param.\r\n"); } } }
void scheduler_init(scheduler_t* scheduler, const scheduler_conf_t* config, const mavlink_stream_t* mavlink_stream) { // Init dependency scheduler->mavlink_stream = mavlink_stream; // Init schedule strategy scheduler->schedule_strategy = config->schedule_strategy; // Init debug mode scheduler->debug = config->debug; // Allocate memory for the task set scheduler->task_set = malloc( sizeof(task_set_t) + sizeof(task_entry_t[config->max_task_count]) ); if ( scheduler->task_set != NULL ) { scheduler->task_set->max_task_count = config->max_task_count; } else { print_util_dbg_print("[SCHEDULER] ERROR ! Bad memory allocation\r\n"); scheduler->task_set->max_task_count = 0; } scheduler->task_set->task_count = 0; scheduler->task_set->current_schedule_slot = 0; print_util_dbg_print("[SCHEDULER] Init\r\n"); }
static mode_flag_armed_t get_armed_flag(remote_t* remote) { const remote_mode_t* remote_mode = &remote->mode; mode_flag_armed_t armed = remote_mode->current_desired_mode.ARMED; // Get armed flag if( remote_get_throttle(remote) < -0.95f && remote_get_yaw(remote) > 0.9f && remote_get_pitch(remote) > 0.9f && remote_get_roll(remote) > 0.9f ) { // Both sticks in bottom right corners => arm print_util_dbg_print("Arming!\r\n"); armed = ARMED_ON; } else if ( remote_get_throttle(remote) < -0.95f && remote_get_yaw(remote) < -0.9f && remote_get_pitch(remote) > 0.9f && remote_get_roll(remote) < -0.9f ) { // Both sticks in bottom left corners => disarm print_util_dbg_print("Disarming!\r\n"); armed = ARMED_OFF; } else { // Keep current flag } return armed; }
DSTATUS disk_initialize (BYTE pdrv) { DSTATUS stat = STA_NOINIT; int result; actual_status = STA_NOINIT; uint8_t drive_num; if (pdrv==0) { drive_num = MMC; } else { return STA_NOINIT; } switch (drive_num) { case ATA : //result = ATA_disk_initialize(); print_util_dbg_print("NO SUPPORTED! ATA init!\r\n"); // translate the result code here result = RES_ERROR; stat = STA_NODISK; break; case MMC : //result = MMC_disk_initialize(); result = sd_spi_init(); if (result) { stat = 0; actual_status = 0; } else { stat = STA_NOINIT; } break; case USB : //result = USB_disk_initialize(); print_util_dbg_print("NO SUPPORTED! USB init!\r\n"); // translate the result code here result = RES_ERROR; stat = STA_NODISK; break; } return stat; }
void neighbors_selection_extrapolate_or_delete_position(neighbors_t *neighbors) { int32_t i, ind, ind_sup; uint32_t delta_t; uint32_t actual_time = time_keeper_get_millis(); for (ind = 0; ind < neighbors->number_of_neighbors; ind++) { delta_t = actual_time- neighbors->neighbors_list[ind].time_msg_received; if (delta_t >= NEIGHBOR_TIMEOUT_LIMIT_MS) { print_util_dbg_print("Suppressing neighbor number "); print_util_dbg_print_num(ind,10); print_util_dbg_print("\r\n"); // suppressing element ind for (ind_sup = ind; ind_sup < (neighbors->number_of_neighbors - 1); ind_sup++) { neighbors->neighbors_list[ind_sup] = neighbors->neighbors_list[ind_sup + 1]; } (neighbors->number_of_neighbors)--; } else if (delta_t > ORCA_TIME_STEP_MILLIS) { // extrapolating the last known position assuming a constant velocity for(i = 0; i < 3; i++) { neighbors->neighbors_list[ind].extrapolated_position[i] = neighbors->neighbors_list[ind].position[i] + neighbors->neighbors_list[ind].velocity[i] *((float)delta_t/1000); } //print_util_dbg_print("Extrapolated position (x100):"); //print_util_dbg_print_num(neighbors->neighbors_list[ind].extrapolated_position[0],10); //print_util_dbg_print(", "); //print_util_dbg_print_num(neighbors->neighbors_list[ind].extrapolated_position[1],10); //print_util_dbg_print(", "); //print_util_dbg_print_num(neighbors->neighbors_list[ind].extrapolated_position[2],10); //print_util_dbg_print(")"); } else { // taking the latest known position for (i = 0; i < 3; i++) { neighbors->neighbors_list[ind].extrapolated_position[i] = neighbors->neighbors_list[ind].position[i]; } //print_util_dbg_print("Last known position (x100):"); //print_util_dbg_print_num(neighbors->neighbors_list[ind].extrapolated_position[0],10); //print_util_dbg_print(", "); //print_util_dbg_print_num(neighbors->neighbors_list[ind].extrapolated_position[1],10); //print_util_dbg_print(", "); //print_util_dbg_print_num(neighbors->neighbors_list[ind].extrapolated_position[2],10); //print_util_dbg_print(")"); } } }
bool scheduler_add_task(scheduler_t* scheduler, uint32_t repeat_period, task_run_mode_t run_mode, task_timing_mode_t timing_mode, task_priority_t priority, task_function_t call_function, task_argument_t function_argument, uint32_t task_id) { bool task_successfully_added = false; task_set_t* ts = scheduler->task_set; // Check if the scheduler is not full if ( ts->task_count < ts->max_task_count ) { // Check if there is already a task with this ID bool id_is_unique = true; for (uint32_t i = 0; i < ts->task_count; ++i) { if ( ts->tasks[i].task_id == task_id ) { id_is_unique = false; break; } } // Add new task if ( id_is_unique == true ) { task_entry_t* new_task = &ts->tasks[ts->task_count]; new_task->call_function = call_function; new_task->function_argument = function_argument; new_task->task_id = task_id; new_task->run_mode = run_mode; new_task->timing_mode = timing_mode; new_task->priority = priority; new_task->repeat_period = repeat_period; new_task->next_run = time_keeper_get_micros(); new_task->execution_time = 0; new_task->delay_max = 0; new_task->delay_avg = 0; new_task->delay_var_squared = 0; ts->task_count += 1; task_successfully_added = true; } else { print_util_dbg_print("[SCHEDULER] Error: There is already a task with this ID\r\n"); task_successfully_added = false; } } else { print_util_dbg_print("[SCHEDULER] Error: Cannot add more task\r\n"); task_successfully_added = false; } return task_successfully_added; }
void print_util_dbg_log_value(const char* msg, int32_t value, char base) { print_util_dbg_print(msg); if (base>1) { print_util_dbg_print_num(value, base); } print_util_dbg_print("\n"); }
void print_util_dbg_init_msg(const char* module_name, bool init_success) { print_util_dbg_print(module_name); if (init_success == true) { print_util_dbg_print(" ok\r\n"); } else { print_util_dbg_print(" INIT ERROR\r\n"); } }
static mav_result_t position_estimation_set_new_home_position(position_estimation_t *pos_est, mavlink_command_long_t* packet) { mav_result_t result; if(pos_est->state->mav_mode.ARMED == ARMED_OFF) { if (packet->param1 == 1) { // Set new home position to actual position print_util_dbg_print("Set new home location to actual position.\r\n"); pos_est->local_position.origin = coord_conventions_local_to_global_position(pos_est->local_position); print_util_dbg_print("New Home location: ("); print_util_dbg_print_num(pos_est->local_position.origin.latitude * 10000000.0f,10); print_util_dbg_print(", "); print_util_dbg_print_num(pos_est->local_position.origin.longitude * 10000000.0f,10); print_util_dbg_print(", "); print_util_dbg_print_num(pos_est->local_position.origin.altitude * 1000.0f,10); print_util_dbg_print(")\r\n"); } else { // Set new home position from msg print_util_dbg_print("[POSITION ESTIMATION] Set new home location. \r\n"); pos_est->local_position.origin.latitude = packet->param5; pos_est->local_position.origin.longitude = packet->param6; pos_est->local_position.origin.altitude = packet->param7; print_util_dbg_print("New Home location: ("); print_util_dbg_print_num(pos_est->local_position.origin.latitude * 10000000.0f,10); print_util_dbg_print(", "); print_util_dbg_print_num(pos_est->local_position.origin.longitude * 10000000.0f,10); print_util_dbg_print(", "); print_util_dbg_print_num(pos_est->local_position.origin.altitude * 1000.0f,10); print_util_dbg_print(")\r\n"); } pos_est->fence_set = false; position_estimation_set_new_fence_origin(pos_est); *pos_est->nav_plan_active = false; result = MAV_RESULT_ACCEPTED; } else { result = MAV_RESULT_TEMPORARILY_REJECTED; } return result; }
void mavlink_message_handler_msg_default_dbg(mavlink_message_t* msg) { if ((msg->sysid == MAVLINK_BASE_STATION_ID)&&(msg->msgid != MAVLINK_MSG_ID_MANUAL_CONTROL)) { print_util_dbg_print("Received message with ID"); print_util_dbg_print_num(msg->msgid, 10); print_util_dbg_print(" from system"); print_util_dbg_print_num(msg->sysid, 10); print_util_dbg_print(" from component"); print_util_dbg_print_num(msg->compid,10); print_util_dbg_print("\r\n"); } }
bool Scheduler::add_task(uint32_t repeat_period, Scheduler_task::task_function_t call_function, Scheduler_task::task_argument_t function_argument, Scheduler_task::priority_t priority, Scheduler_task::timing_mode_t timing_mode, Scheduler_task::run_mode_t run_mode, int32_t task_id) { bool task_successfully_added = false; // Check if the scheduler is not full if (task_count_ < max_task_count_) { if (task_id == -1) { task_id = task_count_; } // Check if there is already a task with this ID bool id_is_unique = true; for (uint32_t i = 0; i < task_count_; ++i) { if (tasks_[i].get_id() == task_id) { id_is_unique = false; break; } } // Add new task if (id_is_unique == true) { tasks_[task_count_++] = Scheduler_task(repeat_period, run_mode, timing_mode, priority, call_function, function_argument, task_id); task_successfully_added = true; } else { print_util_dbg_print("[SCHEDULER] Error: There is already a task with this ID\r\n"); task_successfully_added = false; } } else { print_util_dbg_print("[SCHEDULER] Error: Cannot add more task\r\n"); task_successfully_added = false; } return task_successfully_added; }
void state_telemetry_set_mav_mode(state_t* state, uint32_t sysid, mavlink_message_t* msg) { mavlink_set_mode_t packet; mavlink_msg_set_mode_decode(msg,&packet); // Check if this message is for this system and subsystem // No component ID in mavlink_set_mode_t so no control if ((uint8_t)packet.target_system == (uint8_t)sysid) { print_util_dbg_print("base_mode:"); print_util_dbg_print_num(packet.base_mode,10); print_util_dbg_print(", custom mode:"); print_util_dbg_print_num(packet.custom_mode,10); print_util_dbg_print("\r\n"); mav_mode_t new_mode; new_mode.byte = packet.base_mode; if (new_mode.ARMED == ARMED_ON) { if (state->mav_mode.ARMED == ARMED_OFF) { state_switch_to_active_mode(state, &state->mav_state); } } else { state->mav_state = MAV_STATE_STANDBY; } state->mav_mode.ARMED = new_mode.ARMED; state->mav_mode.MANUAL = new_mode.MANUAL; //state->mav_mode.HIL = new_mode.HIL; state->mav_mode.STABILISE = new_mode.STABILISE; state->mav_mode.GUIDED = new_mode.GUIDED; state->mav_mode.AUTO = new_mode.AUTO; state->mav_mode.TEST = new_mode.TEST; state->mav_mode.CUSTOM = new_mode.CUSTOM; //state->mav_mode_custom = packet.custom_mode; print_util_dbg_print("New mav mode:"); print_util_dbg_print_num(state->mav_mode.byte,10); print_util_dbg_print("\r"); } }
void Data_logging::add_header_name(void) { bool init = true; uint16_t i; init &= console_.write("time"); put_r_or_n(0); for (i = 0; i < data_logging_count_; i++) { data_logging_entry_t* param = &data_log_[i]; init &= console_.write(reinterpret_cast<uint8_t*>(param->param_name), strlen(param->param_name)); if (!init) { break; if (debug_) { print_util_dbg_print("Error appending header!\r\n"); } } else { put_r_or_n(i); } } file_init_ = init; }
bool stabilisation_copter_init(stabilisation_copter_t* stabilisation_copter, stabilisation_copter_conf_t* stabiliser_conf, control_command_t* controls, const imu_t* imu, const ahrs_t* ahrs, const position_estimation_t* pos_est,servos_t* servos) { bool init_success = true; //init dependencies stabilisation_copter->stabiliser_stack = stabiliser_conf->stabiliser_stack; stabilisation_copter->motor_layout = stabiliser_conf->motor_layout; stabilisation_copter->controls = controls; stabilisation_copter->imu = imu; stabilisation_copter->ahrs = ahrs; stabilisation_copter->pos_est = pos_est; stabilisation_copter->servos = servos; //init controller controls->control_mode = ATTITUDE_COMMAND_MODE; controls->yaw_mode = YAW_RELATIVE; controls->rpy[ROLL] = 0.0f; controls->rpy[PITCH] = 0.0f; controls->rpy[YAW] = 0.0f; controls->tvel[X] = 0.0f; controls->tvel[Y] = 0.0f; controls->tvel[Z] = 0.0f; controls->theading = 0.0f; controls->thrust = -1.0f; stabilisation_copter->thrust_hover_point = stabiliser_conf->thrust_hover_point; print_util_dbg_print("[STABILISATION COPTER] initalised.\r\n"); return init_success; }
void Pwm_stm32::write_channel(void) { //select the output period if (TIM_ARR(timer_) > period_) { print_util_dbg_print("should keep slower period.\r\n"); } else { TIM_ARR(timer_) = period_; } if(channel_id_ == PWM_STM32_CHANNEL_1) { //select duty cycle TIM_CCR1(timer_) = duty_cyle_; } else if(channel_id_ == PWM_STM32_CHANNEL_2) { //select duty cycle TIM_CCR2(timer_) = duty_cyle_; } else if(channel_id_ == PWM_STM32_CHANNEL_3) { //select duty cycle TIM_CCR3(timer_) = duty_cyle_; } else if(channel_id_ == PWM_STM32_CHANNEL_4) { //select duty cycle TIM_CCR4(timer_) = duty_cyle_; } }
static void data_logging_add_header_name(data_logging_t* data_logging) { bool init = false; uint16_t i; data_logging_set_t* data_set = data_logging->data_logging_set; for (i = 0; i < data_set->data_logging_count; i++) { data_logging_entry_t* param = &data_set->data_log[i]; int32_t res = f_printf(&data_logging->fil,param->param_name); if (res == EOF) { if (data_logging->debug) { print_util_dbg_print("Error appending header!\r\n"); } init = false; } else { init = true; data_logging_put_r_or_n(data_logging, i); } } data_logging->file_init = init; }
bool servo_mix_ywing_init(servo_mix_ywing_t* mix, const servo_mix_ywing_conf_t* config, const torque_command_t* torque_command, const thrust_command_t* thrust_command, Servo* motor, Servo* flap_top, Servo* flap_right, Servo* flap_left) { bool init_success = true; // Init dependencies mix->torque_command = torque_command; mix->thrust_command = thrust_command; mix->motor = motor; mix->flap_top = flap_top; mix->flap_right = flap_right; mix->flap_left = flap_left; // Init parameters mix->flap_top_dir = config->flap_top_dir; mix->flap_right_dir = config->flap_right_dir; mix->flap_left_dir = config->flap_left_dir; mix->min_thrust = config->min_thrust; mix->max_thrust = config->max_thrust; mix->min_deflection = config->min_deflection; mix->max_deflection = config->max_deflection; print_util_dbg_print("[SERVOS MIX YWING] initialised \r\n"); return init_success; }
bool state_init(state_t *state, state_t* state_config, const analog_monitor_t* analog_monitor) { bool init_success = true; // Init dependencies state->analog_monitor = analog_monitor; // Init parameters state->autopilot_type = state_config->autopilot_type; state->autopilot_name = state_config->autopilot_name; state->mav_state = state_config->mav_state; state->mav_mode = state_config->mav_mode; state->source_mode = state_config->source_mode; state->mav_mode_custom = CUSTOM_BASE_MODE; state->simulation_mode = state_config->simulation_mode; init_success &= battery_init(&state->battery,state_config->battery.type,state_config->battery.low_level_limit); if (state->simulation_mode == HIL_ON) { // state->mav_mode |= MAV_MODE_FLAG_HIL_ENABLED; state->mav_mode.HIL = HIL_ON; } else { // state->mav_mode |= !MAV_MODE_FLAG_HIL_ENABLED; state->mav_mode.HIL = HIL_OFF; } state->fence_1_xy = state_config->fence_1_xy; state->fence_1_z = state_config->fence_1_z; state->fence_2_xy = state_config->fence_2_xy; state->fence_2_z = state_config->fence_2_z; state->out_of_fence_1 = false; state->out_of_fence_2 = false; state->nav_plan_active = false; state->in_the_air = false; state->reset_position = false; state->last_heartbeat_msg = time_keeper_get_time(); state->max_lost_connection = state_config->max_lost_connection; state->connection_lost = false; state->first_connection_set = false; state->msg_count = 0; state->remote_active = state_config->remote_active; print_util_dbg_print("[STATE] Initialized.\r\n"); return init_success; }
void lsm330dlc_init(void) { if(twim_probe(&AVR32_TWIM0, LSM330_ACC_SLAVE_ADDRESS) == STATUS_OK) { print_util_dbg_print("LSM330 sensor found (0x18) \r\n"); } else { print_util_dbg_print("LSM330 sensor not responding (0x18) \r\n"); return; } lsm330dlc_acc_init(); lsm330dlc_gyro_init(); lsm330dlc_get_acc_config(); lsm330dlc_get_gyro_config(); }
void simulation_switch_from_reality_to_simulation(simulation_model_t *sim) { print_util_dbg_print("Switching from reality to simulation.\r\n"); simulation_reset_simulation(sim); simulation_calib_set(sim); sim->pos_est->init_gps_position = false; }
void position_estimation_reset_home_altitude(position_estimator_t *pos_est) { int32_t i; // reset origin to position where quad is armed if we have GPS if (pos_est->init_gps_position) { pos_est->local_position.origin.longitude = pos_est->gps->longitude; pos_est->local_position.origin.latitude = pos_est->gps->latitude; pos_est->local_position.origin.altitude = pos_est->gps->altitude; pos_est->local_position.timestamp_ms = pos_est->gps->time_last_msg; pos_est->last_gps_pos = pos_est->local_position; } //else //{ //pos_est->local_position.origin.longitude = HOME_LONGITUDE; //pos_est->local_position.origin.latitude = HOME_LATITUDE; //pos_est->local_position.origin.altitude = HOME_ALTITUDE; //} // reset barometer offset bmp085_reset_origin_altitude(pos_est->barometer, pos_est->local_position.origin.altitude); //pos_est->barometer->altitude_offset = -pos_est->barometer->altitude - pos_est->local_position.pos[2] + pos_est->local_position.origin.altitude; pos_est->init_barometer = true; print_util_dbg_print("Offset of the barometer set to the GPS altitude, offset value of:"); print_util_dbg_print_num(pos_est->barometer->altitude_offset,10); print_util_dbg_print(" = -"); print_util_dbg_print_num(pos_est->barometer->altitude,10); print_util_dbg_print(" - "); print_util_dbg_print_num(pos_est->local_position.pos[2],10); print_util_dbg_print(" + "); print_util_dbg_print_num(pos_est->local_position.origin.altitude,10); print_util_dbg_print("\r\n"); // reset position estimator pos_est->last_alt = 0; for(i = 0;i < 3;i++) { pos_est->last_vel[i] = 0.0f; pos_est->local_position.pos[i] = 0.0f; pos_est->vel[i] = 0.0f; pos_est->vel_bf[i] = 0.0f; } }
void print_util_dbg_sep(char c) { for (uint8_t i = 0; i < 80; ++i) { deb_stream->put(deb_stream->data, c); } print_util_dbg_print("\r\n"); }
bool fat_fs_mounting_mount(fat_fs_mounting_t* fat_fs_mounting, bool debug) { bool success = false; FRESULT fr = FR_NO_FILESYSTEM; if (!fat_fs_mounting->sys_mounted) { if ((fr != FR_OK) && (fat_fs_mounting->loop_count < 10)) { fat_fs_mounting->loop_count += 1; } if (fat_fs_mounting->loop_count < 10) { fr = f_mount(&fat_fs_mounting->fs, "1:", 1); //fr = f_mount(&fat_fs_mounting->fs, 0, 1); if (fr == FR_OK) { fat_fs_mounting->sys_mounted = true; success = true; } else { fat_fs_mounting->sys_mounted = false; success = false; } if (debug) { if (fr == FR_OK) { print_util_dbg_print("[FAT] SD card mounted\r\n"); } else { print_util_dbg_print("[FAT] [ERROR] Mounting"); fat_fs_mounting_print_error_signification(fr); } } } } return success; }
bool Data_logging::add_field(const double* val, const char* param_name, uint32_t precision) { bool add_success = true; if (val == NULL) { print_util_dbg_print("[DATA LOGGING] Error: Null pointer!"); add_success &= false; } else { if (data_logging_count_ < config_.max_data_logging_count) { if (strlen(param_name) < MAVLINK_MSG_PARAM_SET_FIELD_PARAM_ID_LEN) { data_logging_entry_t* new_param = &data_log_[data_logging_count_]; new_param->param = val; strcpy(new_param->param_name, param_name); new_param->data_type = MAVLINK_TYPE_DOUBLE; new_param->precision = precision; data_logging_count_ += 1; add_success &= true; } else { print_util_dbg_print("[DATA LOGGING] Error: Param name too long.\r\n"); add_success &= false; } } else { print_util_dbg_print("[DATA LOGGING] Error: Cannot add more logging param.\r\n"); add_success &= false; } } return add_success; }